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移动 机 器 人 自主 导航 是 机 器 人 领域 的 热点 研究 内 容 。 本 书 首先 介绍 了 基于 机 器 视觉 
感知 机 制 的 自主 移动 机 器 人 导航 的 理论 与 研究 ， 重 点 介绍 了 基于 地 图 、 地 图 构建 以 及 无 
地 图 环境 下 的 基于 视觉 传感器 的 机 器 人 导航 算法 ,并 给 出 了 实际 应 用 中 基于 视觉 的 机 器 
人 先进 导航 算法 的 实现 。 本 书 详细 阐述 了 实际 应 用 中 基于 视觉 的 自主 导航 算法 以 及 
SLAM 问题 ， 提 出 了 利用 视觉 感知 如 何 实现 子 目标 驱动 的 导航 概念 ， 提 出 了 利用 模糊 逻 
辑 进 行 基于 视觉 的 机 器 人 路 径 跟踪 概念 ， 以 及 如 何 利 用 基于 微型 控制 器 的 传感器 系统 研 
制 实验 室 环境 下 的 低 成 本 机 器 人 。 

本 书 可 作为 从 事 机 器 人 研究 ， 尤 其 是 移动 机 器 人 方面 的 研究 人 员 的 参考 书 ， 也 可 作 
为 高 等 院 校 自 动 化 、 计 算 机 等 相关 专业 研究 生 以 及 教师 的 参考 用 书 
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译 者 F 


移动 机 器 人 自主 导航 是 机 器 人 领域 的 热点 研究 内 容 。 世 界 范 围 内 开展 了 大 
量 该 领域 的 广泛 研究 ， 且 已 得 出 一 些 非常 有 价值 和 应 用 前 景 的 研究 成 果 ， 这 对 
于 移动 机 器 人 在 工业 、 社 会 服务 业 、 空 间 探索 等 多 个 领域 有 着 重要 意义 。 

本 书 首先 介绍 了 基于 机 器 视觉 感知 机 制 的 自主 移动 机 器 人 导航 的 理论 与 研 
究 。 重 点 全 面 ， 综 合 介 绍 了 基于 地 图 、 地 图 构建 以 及 无 地 图 坏 境 下 的 基于 视觉 
传 感 顺 的 机 器 人 导航 算法 ， 并 给 出 了 实际 应 用 中 基于 视觉 的 机 器 人 先进 导航 算 
法 的 实现 。 

本 书 是 专门 研究 基于 机 器 视觉 感知 的 自主 移动 机 器 人 导航 理论 的 著作 。 传 
统 的 机 器 人 导航 系统 主要 利用 声 纳 、 红 外 GPS (全 球 定位 系统 ) 以 及 激光 传 感 
器 等 ， 这 些 方法 都 由 于 传感器 本 身 特性 或 成 本 等 限制 而 存在 一 定 程度 的 不 足 。 
而 视觉 感知 已 逐步 代 蔡 了 上 述 传感器 ， 可 降低 系统 成 本 ， 同 时 提高 系统 的 智能 
性 、 灵 活性 和 重 棒 性 。 

本 书 详细 阐述 了 实际 应 用 中 基于 视觉 的 自主 导航 算法 以 及 SLAM 问题 ， 提 
出 了 利用 视觉 感知 如 何 实现 子 目 标 驱 动 的 导航 概念 ， 同 时 提出 了 利用 模糊 逻辑 
进行 基于 视觉 的 机 器 人 路 径 跟 踪 概 念 ， 以 及 如 何 利用 基于 微型 控制 器 的 传感器 
系统 研制 实验 室 环境 下 的 低 成 本 机 器 人 。 书 中 还 介绍 了 利用 现 有 机 器 人 与 外 设 
集成 实现 低 成 本 机 器 人 的 成 功 案例 。 

本 书 的 最 大 亮点 在 于 ， 详细 介绍 了 在 32 位 Windows 环境 下 如 何 具体 实现 基 
于 视觉 的 导航 模块 ， 另 外 还 讨论 了 如 何 采 用 两 个 摄像 头 实现 基于 视觉 的 SLAM 
系统 概念 。 

本 书 主要 由 连 晓 峰 翻 译 、 审 校 、 整 理 ， 并 对 原 书 中 的 错误 进行 了 注释 ， 王 
WMR, SEY. FRA, WR, ERR, RH, EFE, KRE, FE, ERZ., 
张 子 康 、 王 伟 等 人 也 参与 了 部 分 翻译 工作 。 

本 书 可 作为 从 事 机 器 人 研究 ， 尤 其 是 移动 机 器 人 方面 的 研究 人 员 的 参考 书 ， 
也 可 作为 高 等 院 校 自 动 化 、 计 算 机 等 相关 专业 研究 生 及 教师 的 参考 用 书 。 

限于 译 者 的 经 验 和 水 平 ， 书 中 难免 存在 缺点 和 错误 ， 敬 请 广大 读者 批评 
指正 。 
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本 书 致力 于 采用 基于 计算 机 视觉 的 感知 机 制 进 行 移动 机 器 人 自主 导航 的 理 
论 与 开发 。 传 统 的 机 器 人 导航 系统 利用 如 声 纳 、 红 外 、GPS (全 球 定位 系统 )、 
激光 等 传统 传 感 问 ， 而 这 些 传感器 具有 如 物理 局 限 性 或 成 本 高 等 一 些 缺点 。 视 
觉 感知 作为 一 种 常用 的 替代 方式 ， 可 减少 整体 成 本 ， 维 持 高 度 智 能 性 、 灵 活性 
以 及 鲁 棒 性 。 

第 1 章 详细 阐述 了 移动 机 器 人 自主 导航 的 基本 概念 ， 并 重点 介绍 了 采用 视 
觉 作 为 感知 机 制 。 在 此 ， 对 室内 、 室 外 环境 下 基于 视觉 的 导航 研究 工作 进行 了 
大 致 分 类 。 之 后 ， 介 绍 了 不 同形 式 的 障碍 物 检测 和 避 障 方法 。 第 2 章 讨论 了 如 
何 成 功 实 现 外 围 设备 与 现 有 移动 机 器 人 的 实际 接口 ， 在 此 详细 介绍 了 通过 以 中 
断 驱 动 模式 的 串口 通信 实现 KOALA 机 器 人 与 外 设 的 接口 。 第 3 章 详细 介绍 了 一 
种 基于 视觉 的 机 器 人 导航 策略 ， 其 中 采用 一 个 基于 子 目 标的 机 制 来 沿 最 短路 径 
达到 最 终 目 标 ， 同 时 还 实现 期 望 的 避 障 。 该 策略 采用 一 个 两 层 架 构 ， 其 中 视觉 
传感器 工作 于 第 一 层 ， 而 基于 红外 传感器 的 避 障 机 制 工作 在 第 二 层 。 

第 4 章 讨 论 了 如 何在 实验 室 开发 一 个 具有 特殊 功能 的 低 成 本 机 器 人 ， 重 点 
是 开发 两 个 基于 微 控 制 器 的 机 器 人 传感器 系统 ， 包 括 : 开发 一 个 具有 动态 距离 
增强 功能 的 红外 测 距 系统 ; 采用 开关 模式 同步 检测 技术 开发 一 个 光学 接近 检测 
系统 。 在 第 5 章 ， 一 步 步 地 从 简单 模块 逐步 发 展 为 复杂 模块 ， 讨 论 了 如 何在 32 
位 Windows 环境 下 实际 实现 一 个 基于 视觉 的 导航 子 程序 。 

在 第 6 章 和 第 7 章 讨 论 了 在 移动 机 器 人 导航 中 集成 模糊 逻辑 的 问题 。 其 中 ， 
首先 讨论 了 如 何 开 发 一 个 室内 路 径 跟 踪 的 基于 视觉 的 导航 机 制 ， 在 此 基于 视觉 
的 模糊 导航 与 基于 红外 的 模糊 避 障 机 制 相 结合 。 第 7 章 首先 介绍 了 移动 机 器 人 
基于 EKF 的 SLAM 概念 ， 然 后 讨论 了 一 个 更 复杂 的 场景 ， 其 中 在 具有 不 正确 或 
不 确定 的 传感器 统计 信息 情况 下 ， 可 有 效 利 用 模糊 或 模糊 神经 网 络 监督 来 提高 
基于 EKF 的 SLAM 性 能 。 第 8 章 讨 论 了 如 何 实际 实现 一 个 基于 双 摄 像 头 的 视觉 
系统 以 实现 室内 环境 下 的 SLAM。 

Je UR, 4 £95.45, PÈ 
Amitava Chatterjee 

Anjan Rakshit 

N. Nirmal Singh 

2012 年 9 月 
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第 1 章 移动 机 带 人 导航 


摘要 : 本 章 介 绍 了 移动 机 器 人 自主 导航 的 基本 概念 ， 以 及 利用 视觉 传感器 
作为 感知 机 制 以 实现 预期 目标 的 实用 性 。 本 章 还 讨论 了 在 室内 和 室外 环境 下 基 
于 视觉 的 导航 分 类 ， 并 且 还 介绍 了 在 该 研究 领域 下 各 种 显著 的 研究 方向 ， 以 及 
多 种 不 同 的 障碍 物 检 测 和 避 障 方式 。 


1.1 自主 移动 机 器 人 导航 


近年 来 ， 机 器 人 领域 的 一 些 先进 技术 的 发 展 进步 已 对 许多 工业 生产 和 社会 
发 展 作出 了 巨大 贡献 。 如 今 ， 机 器 人 系统 已 在 工厂 自动 化 生产 、 监 控 系 统 、 质 
量 控制 系统 、AGV( 自 主导 引 车 ) 、 抗 灾 和 医疗 辅助 等 领域 得 到 广泛 应 用 。 越 来 
越 多 的 机 器 人 应 用 正在 改善 我 们 的 日 常生 活 ， 同 时 在 执行 各 种 不 同 任务 方面 ， 
机 器 人 也 前 所 未 有 地 得 到 了 更 多 关注 ”。 对 于 很 多 这 样 的 应 用 ,机 器 人 的 自主 
移动 性 是 一 个 非常 关键 的 问题 。 自 主 移动 机 器 人 是 指 在 没有 人 工 导 引 的 结构 
化 或 非 结 构 化 环境 中 能 完成 预期 任务 的 机 器 人 。 一 个 完全 自主 的 移动 机 器 人 应 
具备 以 下 能 力 : 

1) 能 够 获取 环境 的 相关 信息 ; 

2) 能 够 在 没有 人 为 干预 的 情况 下 长 时 间 工 作 ; 

3) 无 需 人 工 辅助 能 够 在 整个 工作 环境 中 整体 或 部 分 运动 ; 

4) 除非 是 机 器 人 的 设计 规范 ， 和 否则 能 够 避免 出 现 危 害 人 类 、 财 产 或 其 自身 
的 情况 。 

一 个 自主 移动 机 器 人 还 应 能 够 学 习 或 获取 新 的 能 力 ， 如 调整 策略 以 完成 任 
务 或 自 适 应 环境 的 变化 。 


1.2 为 何 要 视觉 导航 


视觉 是 能 够 让 我 们 人 类 获取 客观 世界 相关 信息 的 一 种 感知 ， 确 切 地 说 ， 这 
也 是 人 类 最 依赖 的 一 种 感知 。 近 年 来 ， 能 够 提取 上 述 信息 的 计算 机 视觉 技术 已 
得 到 不 断 地 发 展 和 完善 。 虽 然 视觉 信息 处 理 的 计算 量 庞大 , 但 随 着 处 理 速度 更 
快 且 价格 更 低 的 处 理 器 不 断 地 发 展 ， 对 于 各 种 复杂 任务 ， 基 于 视觉 的 移动 机 器 


2 基于 视觉 的 自主 机 器 人 导航 





人 实时 导航 系统 正 逐 渐 成 为 现实 ， 并 且 在 该 研究 领域 ， 也 前 所 未 有 地 吸引 了 更 
ZI TE Um, 

用 于 机 器 人 导航 的 其 他 传感器 包括 红外 传感器 、 声 纳 传感器 、 激 光 测 距 仪 、 
位 置 感知 装置 (PSD ) 以 及 惯性 传感器 等 。 红 外 传感器 的 应 用 非常 有 限 ， 常 用 作 接 
近 检 测 器 ， 作 为 测 距 仪 的 主要 缺点 在 于 测 距 范围 有 限 且 易 受 环境 光 的 干扰 ， 另 
外 还 具有 非 线 性 特性 且 反 射 率 取决 于 目标 物体 的 表面 ”。 声 纳 传感器 计算 量 较 
小 且 易 于 读数 ， 但 测量 数据 由 于 环境 干扰 而 可 靠 性 较 低 。 声 纳 测 距 仪 可 测量 与 
物体 之 间 的 距离 ， 但 由 于 其 波 东 较 宽 而 导致 角度 分 辩 率 较 差 ”。 与 声 纳 传感器 
相 比 ， 激 光 测 距 仪 可 提供 更 高 可 靠 性 的 瞬时 测量 、 更 精确 的 测 距 精 度 和 角 分 辨 
率 以 及 更 佳 的 方向 分 辨 率 , 但 成 本 也 更 高 。 激 光 传 感 器 还 可 提取 除了 距离 之 外 
的 信息 ， 比 如 通常 利用 激光 扫描 仪 的 功能 来 提取 拓扑 信息 ， 以 确定 物体 表面 的 
纹理 结构 以 及 精确 的 距离 信息 。 激 光 测 距 仪 的 一 个 缺点 是 激光 扫描 可 能 会 检测 
不 到 透明 物体 (如 玻璃 和 窗户 )。 惯 性 导航 传感器 (如 加 速度 计 和 陀螺 仪 ) 可 提供 
运动 车 辆 的 朝向 和 轨迹 测量 , 但 无 法 提供 车 辆 运行 环境 中 的 障碍 物 信 息 。GPS 
(全 球 定位 系统 ) 是 目前 在 导航 系统 中 最 常用 的 辅助 工具 之 一 ，GPS 可 提供 绝对 
或 相对 的 实时 位 置 数据 ， 但 与 接近 操作 的 一 般 需 求 相 比 ， 其 精确 性 和 带宽 有 限 。 
GPS 的 性 能 可 能 会 受到 卫星 “视线 "遮挡 的 影响 ， 从 而 导致 其 精确 性 和 更 新 率 下 
降 ”。 这 些 测 距 传感器 都 难以 检测 地 面 上 小 型 或 扁平 的 物体 ， 也 无 法 区 别 不 同 
类 型 的 地 表 。 尽 管 这 些小 目标 和 不 同类 型 的 地 表 难 以 通过 测 距 传感器 进行 检测 ， 
但 大 多 数 情况 下 都 可 由 一 个 被 动 传感器 (如 摄像 头 ) 来 检测 。 一 个 视觉 系统 可 看 
作 一 种 被 动 传感器 ， 与 看 作 主 动 传 感 器 的 红外 、 激 光 和 声 纳 等 传感器 相 比 ， 其 
具有 一 些 基 本 优势 。 被 动 传感器 (如 摄像 头 ) 通 过 发 射 光 或 波 来 获取 数据 而 不 
会 改变 环境 ， 并 且 所 获得 的 图 像 中 包含 更 多 的 信息 (如 实质 性 、 空 间 和 时 间 信 
息 ) 。 因 此 ， 上 述 所 有 传感器 所 获取 的 物理 环境 相关 信息 都 要 少 于 摄像 头 所 获得 
的 信息 。 同 时 ， 随 着 更 快 、 更 廉价 的 计算 能 力 的 不 断 提 高 ， 正 逐渐 设计 出 基于 
视觉 的 真实 世界 导航 系统 。 摄 像 头 价格 低廉 ， 即 使 是 最 贵 的 摄像 头 也 相对 经 济 、 
实惠 。 因 此 ， 视觉 作为 移动 机 器 人 的 一 种 感知 方法 提供 了 非常 具有 吸引 力 的 解 
决 方案 。 





13 ”基于 视觉 的 导航 


基于 视觉 的 机 器 人 导航 是 一 种 主要 利用 视觉 传感器 来 引导 移动 机 器 人 避 开 
静态 (或 可 能 是 动态 ) 障碍 物 到 达 预 期 目的 地 或 在 环境 中 沿 一 条 预期 路 径 运 动 的 
技术 。 一 般 来 说 ， 基 于 视觉 的 机 器 人 具有 一 个 能 够 感知 外 界 环境 的 视觉 系统 。 
通常 ， 一 个 基于 视觉 的 自主 机 器 人 在 视觉 系统 中 具有 五 个 主要 组 成 部 分 : 
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1. 地 图 

系统 需要 外 界 环境 的 一 些 内 部 表征 或 知识 以 完成 目标 驱动 的 任务 。 
2. 数据 获取 

系统 通过 摄像 头 来 采集 图 像 。 

3， 特 征 提取 


从 采集 的 输入 图 像 中 提取 显著 特征 ， 如 边缘 、 纹 理 和 颜色 。 

4. 路 标识 别 

根据 某 些 预 设 标准 ， 系 统 在 观测 图 像 中 的 特征 和 内 存 中 预存 的 预期 路 标 之 
间 寻 找 可 能 存在 的 匹配 信息 

5. Ave ti 

在 自 定 位 阶段 ， 计 算 作为 检测 路 标 和 之 前 位 置 的 一 个 函数 的 机 器 人 当前 位 
置 ， 然 后 系统 推导 出 机 器 人 的 运动 路 径 。 机 器 人 的 运动 过 程 可 仅仅 进行 避 障 和 / 
或 是 以 目标 驱动 ， 

移动 机 器 人 的 导航 问题 大 多 数 情况 下 可 分 为 以 下 四 个 子 问 题 ” : 

l. 环境 感知 

感知 环境 并 将 其 抽象 为 一 系列 特征 

2. 路径 规 划 

利用 特征 来 构建 机 器 人 应 达到 的 一 系列 目标 点 

3. 路 径 生 成 

接 下 来 ， 通 过 一 系列 目标 点 来 获得 一 条 路 径 

4 路径 跟踪 

利用 控制 器 来 保证 移动 机 器 人 沿 预 期 路 径 运动 。 

一 般 来 说 ， 开 发 一 个 能 够 检测 并 避 开 障碍 的 基于 视觉 的 机 器 人 系统 是 相当 
复杂 的 ， 这 是 由 于 从 包括 机 器 人 和 障碍 物 的 现场 视频 流 中 提取 信息 ， 并 利用 尽 
可 能 少 的 计算 处 理 来 实现 预期 实时 性 能 是 一 项 非常 复杂 的 任务 。 在 过 去 的 20 年 
中 ， 未 知 环境 下 机 器 人 运动 的 问题 得 到 了 广泛 关注 。 机 器 人 应 能 够 以 智能 方式 
避 开 可 能 遇 到 的 各 种 不 同类 型 的 障碍 物 。 相 应 地 ， 大 量 研究 工作 致力 于 利用 计 
算 机 视觉 实现 通过 对 感知 数据 进行 逻辑 分 析 来 实现 避 障 的 具有 导航 能 力 的 基于 
视觉 的 自主 移动 机 器 人 系统 。 大 多 数 这 些 研 究 的 主要 目标 是 定位 静态 或 动态 障 
碍 物 ， 从 而 可 规划 出 一 条 合适 的 路 径 以 使 得 机 器 人 绕 开 障碍 物 ， 并 最 终 按 所 规 
划 的 路 径 运 动 。 利 用 视觉 感知 的 室内 和 室外 导航 已 成 为 移动 机 器 人 领域 中 两 个 
EY WEE 7r i o 


13.1 基于 视觉 的 室内 导航 
基于 视觉 的 室内 导航 机 制 大 致 可 分 为 三 类 : 
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1) 基于 地 图 的 导航 ; 

2) 基于 地 图 构建 的 导航 ; 

3) 无 地 图 导航 。 

1.3.1.1 基于 地 图 的 导航 

在 基于 地 图 的 导航 过 程 中 ， 系 统 已 具有 环境 的 先 验 知识 ， 并 利用 地 图 信息 进 
行 导 航 。 这 些 环境 地 图 的 形式 主要 有 几何 模型 、 拓 扑 地 图 或 图 像 序列 “" 。 在 早 
期 利用 地 图 的 方法 中 将 环境 细 化 为 不 同 程度 ,分 别 为 “占据 栅 格 地 图 ”"”、“ 虚 拟 力 
场 " 或 “S 地 图 ”等 形式 。 这 些 方法 非常 容易 受到 感知 误差 的 影响 ， 在 参考 文献 
[12 ] 中 对 该 问题 进行 了 详细 阐述。 其 中 ， 一 个 重要 假设 是 应 对 传感器 测量 的 不 确 
定性 考虑 一 定 容 差 '” 。 这 些 方法 都 称 为 “绝对 定位 ”， 之 后 对 这 些 研究 问题 进行 改 
进 ， 采 用 ”* 增 量 定位 "来 解决 。 在 此 ， 假 设 已 知 机 器 人 位 置 的 似 然 信息 ， 在 导航 过 
程 中 通过 利用 视觉 工具 的 观察 不 断 修 正 这 些 信 息 ， 并 在 随后 的 导航 中 采取 必要 措 
jis FINALE!) 系统 正 是 基于 上 述 概念 ， 其 中 当 某 个 路 标 与 一 个 图 像 特征 匹配 时 ， 
利用 空间 的 几何 表示 以 及 机 器 人 位 置 的 统计 模型 不 确定 性 ， 采 用 卡尔 曼 滤波 方法 
来 更 新 机 器 人 位 置 的 均值 和 协 方差 矩阵 。 另 一 类 方法 是 基于 空间 的 拓扑 表示 ， 其 
中 如 在 NEURO-NAV ”中 采用 了 一 组 神经 网 络 ， 或 如 在 称 为 FUZZY-NAVL 的 系统 
中 采用 比 NEURO-NAV 更 复杂 的 监督 式 模糊 控制 部。 然而 ， 基 于 地 图 的 导航 方法 
的 缺点 在 于 不 易 产生 环境 的 模型 或 地 图 (尤其 是 米 制 地 图 ) 。 

1.3.1.2 基于 地 图 构建 的 导航 

基于 地 图 构建 的 导航 处 理 程序 试图 解决 没有 环境 先 验 信 息 的 机 器 人 开始 运 
动 的 问题 。 机 器 人 首先 探索 周围 环境 并 建立 一 个 内 部 描述 ， 之 后 再 利用 该 内 部 
描述 进行 导航 任务 。 在 该 研究 领域 中 ， 大 部 分 都 是 采用 基于 空间 拓扑 表征 的 方 
法 ， 参 见 参考 文献 [16]，[17]， 并 研究 如 何在 基于 图 的 室 间 描述 中 构建 节点 、 
如 何 区 分 几 个 相 邻 节点 、 如 何 考虑 传感器 不 确定 性 影响 等 一 系列 问题 。 然 而 ， 
这 些 方法 的 主要 缺点 是 难以 识别 之 前 访问 过 的 节点 。 在 该 领域 的 其 他 方法 还 包 
括 基于 占据 栅 格 的 表示 “"” ， 采用 全 景 视觉 的 方法 ”， 以 及 综合 占据 机 格 方法 
和 拓扑 结构 方法 优点 的 方法 ” 。 文 献 中 还 包括 一 些 其 他 类 型 的 基于 地 图 构建 的 
导航 系统 ， 如 基于 可 视 化 声 纳 的 系统 ”或 基于 局 部 地 图 的 系统 ” 。 这 些 系统 都 
是 在 导航 过 程 中 采集 环境 数据 并 构建 一 个 用 于 支持 在 线 安全 导航 的 局 部 地 图 。 
该 局 部 地 图 包括 通常 为 摄像 头 视 场 角 孔 数 的 部 分 环境 中 具体 障 但 物 信 息 和 自由 
空间 数据 信息 。 

1.3.1.3 无 地 图 导航 

无 地 图 导航 方法 属于 一 类 更 有 前 景 的 方法 ， 其 中 导航 过 程 在 没有 任何 地 图 
的 条 件 下 开始 进行 。 这 种 类 型 的 导航 过 程 也 称 为 反应 式 导 航 ， 这 是 通过 观测 、 
特征 /路 标识 别 ( 通 常 是 墙 、 桌 子 、 楼 道 、 墙 角 等 自然 物体 ) 以 及 特征 跟踪 来 在 线 
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提取 环境 的 重要 信息 ， 然 后 利用 导航 算法 对 所 提取 的 相关 有 用 信息 做 出 “反应 ”。 
在 这 些 技术 中 ， 一 些 传统 的 常用 方法 是 采用 光 流 法 ”和 基于 外 观 的 技术 “ 。 
光 流 法 是 模仿 蜜蜂 的 可 视 化 行为 ， 根 据 机 器 人 左右 摄像 头 获取 图 像 之 间 的 速率 
差 来 确定 运动 。 机 器 人 会 朝 图 像 变化 速率 较 小 的 那 一 侧 运动 。 参 考 文献 [24 ] 在 
该 基本 方法 的 基础 上 进一步 改进 ， 开 发 了 一 种 利用 深度 信息 的 基于 光 流 的 导航 
系统 ， 并 且 还 开发 了 一 个 具有 俯仰 、 倾 斜 、 左 右 幅 度 控 制 的 更 加 复杂 的 立体 头 
部 系统 。 一 些 研究 人 员 已 证 明 能 够 获得 准确 深度 信息 的 立体 视觉 与 光 流 分 析 方 
法 相 结合 可 具有 更 好 的 导航 效果 ”” 。 在 参考 文献 [ 29] 中 ， 利 用 立体 信息 与 来 
自立 体 图 像 之 一 的 光 流 相 结合 来 构建 一 个 占据 栅 格 地 图 ， 并 对 一 个 地 面 机 器 人 
执行 实时 导航 策略 。 另 一 方面 ， 基 于 外 观 的 方法 通过 保存 一 系列 图 像 来 增强 环 
境 印 象 ， 这 一 系列 图 像 通 常 是 从 向 下 采样 的 摄像 头 采 集 图 像 所 提取 的 子 窗 口中 
创建 的 ， 然 后 在 任意 给 定时 刻 ， 对 采集 图 像 与 所 有 模板 进行 扫描 来 确定 是 否 与 
其 中 某 个 模板 相 匹配 。 如 果 图 像 匹 配 ， 则 在 导航 中 采取 相应 的 控制 动作 。 在 此 ， 
关键 问题 是 如 何 提高 在 训练 阶段 的 图 像 保存 方法 ， 以 及 随后 的 图 像 匹 配 过 程 。 
不 利用 地 图 进行 环境 识别 主要 有 两 种 方法 ”: 

1) 基于 模型 的 方法 。 利 用 预先 定义 的 对 象 模型 来 识别 复杂 环境 中 的 特征 并 
在 环境 中 自 定位 。 

2) 基于 视图 的 方法 。 该 方法 并 不 从 预存 的 图 像 中 提取 特征 ， 而 是 采用 图 像 
匹配 算法 来 实现 自 定位 。 


1.3.2 基于 视觉 的 室外 导航 


这 些 系统 无 需 对 导航 空间 进行 明确 表示 ， 而 是 通过 识别 在 环境 中 所 观测 到 的 
物体 或 通过 产生 基于 视觉 观测 的 运动 来 跟踪 这 些 物体 ， 根 据 环境 结构 的 程度 ， 室 
外 导航 可 分 为 两 大 类 : 结构 化 环境 下 的 室外 导航 和 非 结 构 化 环境 下 的 室外 导航 。 
在 许多 情况 下 ， 室 内 导航 所 采用 的 地 图 表示 方式 并 不 适用 于 室外 导航 ， 这 是 由 于 
室外 通常 都 是 包含 大 量 物理 区 域 的 大 型 场景 ， 由 此 会 导致 表征 室外 环境 的 信息 量 
急剧 增 大 到 无 法 估量 的 地 步 。 在 结构 化 环境 中 的 室外 导航 主要 是 指 道路 跟踪 ， 即 
检测 路 线 并 持续 导航 的 能 力 。 相 对 于 结构 化 的 室内 空间 ， 室 外 环境 大 多 数 情况 下 
都 是 由 砂 石 、 花 园 、 人 行道 和 街道 组 成 的 。 大 多 数 这 些 元 素 都 具有 不 同 的 颜色 和 
纹理 ， 利 用 这 些 特征 可 以 很 方便 地 进行 室外 导航 。 确 定 导航 区 域 的 第 一 步 就 是 根 
据 视 觉 信息 对 地 形 进 行 分 类 。 迄 今 为 止 ， 在 道路 跟踪 方面 所 报道 的 最 突出 的 研究 
工作 是 NAVLAB 工程 ”| 。NAVLAB 道路 跟踪 算法 包括 三 个 阶段 : 第 一 阶段 ， 对 
每 个 道路 和 非 道路 像素 都 采用 高 斯 分 布 来 进行 颜色 和 纹理 像素 的 综合 分 类 ; 第 二 
阶段 ， 对 道路 像素 执行 霍 夫 变换 和 投票 表决 程序 ， 来 获得 道路 消失 点 和 朝向 参数 ; 
最 后 ， 根 据 确定 的 道路 边缘 对 像素 再 次 分 类 。 在 下 一 幅 图 像 中 重复 执行 上 述 分 类 
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过 程 以 使 得 系统 可 适应 路 况 的 变化 。 对 于 结构 化 环境 下 的 道路 检测 和 跟踪 ， 许 多 
研究 工作 都 是 根据 上 述 相关 概念 进行 的 “”。 在 室外 环境 中 采用 视觉 信息 和 GPS 
数据 是 提高 位 置 估 计 可 靠 性 的 另 一 种 方法 o 在 最 近 的 研究 工作 中 ， 和 研究 人 员 还 
提出 了 一 种 特征 跟踪 与 3D 立体 环境 重 构 相 结合 的 方法 。 在 参考 文献 [38 ] 中 ， 对 于 
非 结 构 化 室外 环境 ， 采 用 立体 视觉 作为 一 种 新 的 导航 策略 。 该 系统 是 基于 一 种 更 
快 且 更 精确 的 角 点 检测 新 方法 。 在 该 方法 中 ，3D 定位 所 检测 的 特征 并 采用 归 一 化 
均 方 差 和 相关 测量 来 进行 特征 跟踪 。 


1.4 研究 现状 


至 今 为 止 ， 在 基于 视觉 的 移动 机 器 人 领域 已 进行 了 大 量 的 研究 工作 ， 且 大 
多 数 研 究 都 集中 于 障碍 物 检 测 、 环 境地 图 构建 、 安 全 路 径 规划 和 楼 道 导 航 。 本 
节 将 对 一 些 已 采用 基于 视觉 导航 的 不 同 导航 方法 进行 概述 ， 即 基于 摄像 头 采集 
的 视觉 信息 ， 且 已 知 或 未 知 环境 的 先 验 信息 。 

“Stanford Cart" 是 一 项 众所周知 的 基于 视觉 的 移动 机 器 人 项 目 ” ， 该 项 目 已 
完成 了 很 长 一 段 时 间 。 该 系统 采用 一 个 安装 在 移动 平台 上 的 摄像 头 ， 将 视频 信 
号 发 送 到 远程 计算 机 ， 然 后 通过 这 些 视 频 信 号 来 处 理 和 控制 机 器 人 的 运动 ， 同 
时 还 具有 一 个 用 于 避 障 和 路 径 确 认 的 规划 融 。 一 旦 接收 到 包含 环境 完整 场景 的 
图 像 ， 系 统 就 会 进行 处 理 。 据 报道 ， 该 处 理 时 间 长 达 1 ~2h。 一 旦 完成 图 像 处 
理 ， 就 会 产生 一 条 规划 路 径 来 引导 机 器 人 实现 避 障 。 该 系统 的 主要 问题 是 信息 
处 理 的 时 间 太 长 。 例 如 ， 阴 影 移 动 会 导致 机 器 人 在 所 构建 的 地 图 中 产生 误差 。 
后 来 ， 通 过 图 像 分 割 与 兴趣 算 子 相 结合 的 思想 来 检测 环境 中 的 不 同 特征 ， 从 而 
解决 上 述 缺 点 ” 。 

在 20 世纪 90 年 代 早期 ，Horswill 研制 开发 了 一 个 称 为 POLLY 的 机 器 
A ,该 机 器 人 采用 单 目 视觉 进行 导航 ， 并 工作 在 一 个 颜色 固定 的 环境 中 。 
在 导航 过 程 中 ，POLLY 采用 一 个 由 用 于 自 定位 的 一 系列 路 标 所 组 成 的 拓扑 地 图 ， 
这 些 路 标 是 在 办 公 室 环境 中 特定 位 置 拍摄 的 一 组 照片 。 为 确定 机 器 人 当前 所 处 
的 位 置 ， 可 通过 将 每 个 路 标 与 实时 视频 数据 相 匹 配 来 进行 路 标 或 位 置 识别 。 在 
该 研究 工作 中 ， 实 际 上 是 采用 一 种 等 效 于 人 工 势 场 法 的 局 部 导航 策略 。 因 此 ， 
机 器 人 很 容易 陷入 局 部 最 小 值 。 这 项 工作 中 所 采用 的 方法 主要 可 分 为 四 步 : P 
滑 图 像 ; 从 前 景 的 梯形 区 域 中 确定 平均 像素 值 ; 将 该 平均 像素 值 作为 地 板 ， 然 
后 从 图 像 底部 开始 标记 每 个 像素 ， 并 对 每 列 进行 扫描 ， 直 到 出 现 错误 匹配 ; 列 
的 高 度 表 明了 机 器 人 与 障 得 物 之 间 的 距离 ， 即 径 向 深度 地 图 。 这 种 方法 的 主要 
问题 在 于 经 常会 将 阴影 误 判 为 障碍 物 ， 这 主要 也 是 由 于 机 器 人 视觉 处 理 系统 过 
于 简单 所 造成 的 。 这 种 处 理 过 程 基于 在 图 像 中 简单 提取 无 纹理 的 地 板 ， 从 而 确 
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定 可 运动 到 的 自由 空间 。 在 以 后 的 几 年 中 ， 研 究 人 员 还 提出 只 具有 一 个 摄像 头 
的 移动 机 器 人 导航 机 制 “” 。 同 时 还 提出 采用 占据 顶 格 的 地 图 构建 框架 和 特征 
位 置 检测 算法 ， 即 通过 单个 摄像 头 在 线 处 理 RGB 彩色 图 像 序列 “ 。 该 系统 并 不 
是 采用 匹配 方法 ， 而 是 计算 每 个 位 置 处 找到 物体 的 概率 。 该 算法 首先 在 当前 图 
像 帧 中 通过 Harris 边缘 和 角 点 检测 器 来 检测 物体 的 轮廓 边缘 。 在 图 像 序列 的 开 
始 处 ,在 占据 栅 格 地 图 中 增加 边缘 特征 ， 对 该 边缘 特征 扫描 可 确定 峰值 。 然 后 ， 
考虑 任意 深度 下 的 所 有 可 能 位 置 ， 将 检测 到 的 特征 投影 到 2D 图 像 平 面 。 根 据 里 
程 计数 据 和 提取 的 图 像 特 征 ， 系 统 定 位 模块 可 计算 机 器 人 位 置 。 利 用 之 前 图 像 
中 边缘 和 特征 的 颜色 或 梯度 有 助 于 增强 在 某 一 特定 位 置 存在 物体 的 可 信和 度 。 室 
内 环境 下 实验 的 网 格 大 小 设置 为 23mm x25mm。 在 连续 图 像 中 ， 人 允许 机 器 人 移 
动 100mm。 由 于 采用 单个 摄像 头 只 能 获取 前 向 信息 ， 但 这 些 信息 对 于 室内 导航 
已 足够 ， 且 性 价 比较 高 。 对 于 室内 未 知 环境 ， 采 用 单 摄像 头 的 其 他 研究 工作 大 
多 集中 于 采用 中 线 跟踪 和 沿 墙 跟踪 的 方法 “ 。 最 近 ， 参 考 文献 [47] 中 提出 了 一 
种 采用 单个 USB 摄像 头 和 笔记 本 电脑 实现 的 轮 式 移动 机 器 人 走廊 中 线 跟踪 方法 。 
在 该 实验 中 ,采集 的 图 像 大 小 为 320 x 240 像素 ， 前 视 场 大 小 为 底部 60cm、 顶 部 
20m， 且 机 各 人 运动 速度 为 0.823km/h。 该 方法 也 采用 堆 夫 变换 来 检测 走 亡 和 墙 
辟 的 边界 。 当 走 亡 中 没有 障碍 物 时 ， 机 器 人 将 沿 走廊 中 线 运 动 。 在 此 所 用 的 障 
碍 物 检测 方法 是 改进 的 Ulrich 法 ”。 这 种 改进 是 忽略 了 由 于 光线 影响 导致 的 障 
碍 物 错误 检测 。 在 参考 文献 L47] 中 ,一 旦 检测 到 任何 障碍 物 ， 则 根据 障碍 物 大 
小 、 与 机 器 人 的 距离 以 及 走廊 宽度 来 决定 是 避 障 还 是 停止 ， 这 些 都 是 由 真实 空 
间 中 的 2D 位 置 和 图 像 中 的 任意 位 置 来 确定 的 。 另 外 还 有 研究 人 员 提 出 了 一 种 基 
于 定性 的 方法 ， 即 利用 一 个 现成 的 未 标定 的 前 向 观测 摄像 头 进行 室内 和 室外 导 
航 ”。 在 此 ， 采 用 一 种 基于 示 教 一 重 现 的 方法 。 在 示 教 阶段 ， 用 户 手动 引导 机 
器 人 治 着 所 规划 的 路 径 运动 。 这 时 ， 机 器 人 采用 Kanade-Lucas-Tomasi ( KLT) 特征 
跟踪 器 “来 选择 和 跟踪 特征 点 ， 并 保存 在 数据 库 中 。 在 重 现 阶段 ， 应 建立 当前 
图 像 中 特征 点 的 坐标 与 示 教 阶段 在 第 一 幅 图 像 中 所 选择 特征 点 的 坐标 之 间 的 对 
应 关系 ， 由 此 来 确定 转弯 命令 。 同 理 ， 在 立体 视觉 中 也 开发 了 一 种 类 似 的 基于 
用 户 经 验 的 示 教 一 重 现 导 航 算法 ” 。 

在 立体 视觉 中 ， 机 器 人 可 通过 两 个 或 多 个 摄像 头 来 测量 距离 信息 ， 也 可 采 
用 声 纳 传感器 。 然 而 ， 使 用 两 个 或 多 个 摄像 头 会 增 大 处 理 成 本 。 据 报道 ， 目 前 
在 一 些 移动 机 器 人 导航 研究 工作 中 已 采用 两 个 或 多 个 摄像 头 “” 。 利 用 两 个 摄 
像 头 为 获得 深度 信息 ， 必 须 已 知 摄像 头 和 机 器 人 头 部 的 相关 几何 信息 。 在 立体 
视觉 中 要 获得 深度 信息 ， 要 求 两 个 摄像 头 的 视线 需 在 场景 中 的 了 P 点 相交 ， 在 该 
点 处 进行 处 理 以 获得 深度 信息 。 基 于 立体 摄像 头 的 系统 在 导航 中 可 用 于 特征 识 
别 、 特 征 跟踪 以 及 实时 计算 3D 特征 点 的 距离 。 这 很 大 程度 上 取决 于 在 一 帧 图 像 
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中 图 像 点 /特征 的 选择 /提取 ， 以 及 随后 在 不 同位 置 和 /或 不 同 朝向 上 采集 的 相同 
场景 连续 图 像 帧 中 对 这 些 点 或 特征 的 跟踪 。 除 非 能 够 识别 并 跟踪 明显 特征 ， 否 
则 基于 特征 的 视觉 系统 无 法 工作 。 获 取 图 像 特征 的 3D 坐标 非常 重要 ， 这 便 
于 计算 像 平 面 上 所 选 特征 与 机 器 人 的 摄像 头 焦点 之 间 的 距离 。 这 些 3D 坐标 对 于 
机 项 人 定位 和 场景 解释 十 分 重要 。 近 年 来 , 已 提出 一 些 利用 鱼 眼 立体 视觉 、 
单 目 视觉 ”、 视 觉 与 声 纳 传感器 相 结 合 ”以 及 从 特征 检测 器 (FD) 接 收 预 处 理 
输入 的 生物 启发 显著 图 (SM ) 等 来 测量 特征 /物体 与 摄像 头 之 间距 离 的 方法 。 在 
参考 文献 [59] 中 ， 两 个 摄像 头 的 FD 和 SM 之 间 的 相互 作用 可 用 于 左右 两 幅 图 像 
中 相应 路 标的 检测 ， 并 可 估计 路 标 方向 和 距离。 一 个 神经 网 络 将 七 个 给 定 标识 
符 (所 有 图 像 中 路 标的 XX 和 YY 坐标、 两 个 摄像 涉 的 平移 角度 和 一 个 共同 的 摄像 
头 倾斜 角 ) 与 路 标的 方向 和 距离 相对 应 。 在 LAGR 机 器 人 中 提出 了 一 种 基于 立体 
视觉 的 栅 格 地 图 构建 方法 来 进行 室外 导航 '”。 除 了 视觉 传感器 ， 还 集成 了 惯性 
导航 单元 、GPS 接收 器 和 前 端 防磁 开关 来 进行 外 界 感知 。 其 中 ， 地 图 构建 方法 
主要 包括 以 下 五 个 步骤 : 

1) 用 一 对 彩色 摄像 头 来 采集 图 像 ; 

2) 根据 摄像 头 的 相对 几何 关系 ， 在 两 幅 图 像 的 匹配 块 中 确定 3D 表征 ; 

3) 转换 每 个 坐标 点 来 确定 机 器 人 的 瞬时 俯仰 角 和 横 滚 角 ， 从 局 部 坐标 系 转 
换 到 全 局 坐标 系 所 估计 的 机 器 人 惯性 导航 和 偏 航 角 ; 

4) 对 地 形 图 进行 求 导 操作 来 寻找 斜率 上 的 突变 ; 

5) 利用 包括 地 形 及 其 导数 估计 的 新 测量 值 来 更 新 全 局 地 图 。 

近年 来 ， 成 功 开发 了 一 种 基于 立体 视觉 的 算法 以 用 于 NASA( 美 国 国家 航空 
航天 局 ) 的 火星 探索 漫游 机 器 人 Mars Exploration Rover 在 具有 潜在 危险 的 地 形 下 
进行 自主 导航 。 该 项 目 中 的 具体 操作 步骤 包括 : 对 摄像 头 采集 图 像 下 采样 256 x 
256 像素 以 减少 计算 时 间 ; 然后 通过 将 第 一 幅 图 像 中 物体 的 外 极 线 水 平 投影 到 另 
一 幅 图 像 上 的 同一 物体 来 对 图 像 对 进行 处 理 ; 接 下 来 计算 两 幅 图 像 的 拉 普 拉 斯 
算 子 ， 并 与 视差 范围 内 选择 的 潜在 匹配 相关 联 ， 对 图 像 中 的 所 有 像素 重复 执行 
上 述 过 程 ， 从 而 使 得 如 果 这 些 像素 的 估计 值 匹配 失败 ， 则 舍弃 ; 最 后 每 个 视差 
值 都 可 利用 摄像 头 几 何 模型 映射 到 一 个 3D 表示 。 

在 开发 基于 视觉 的 机 器 人 导航 策略 过 程 中 ， 采 用 全 方位 视觉 也 是 一 个 非常 
重要 的 方式 。 全 方位 摄像 头 可 对 处 于 任何 位 置 的 机 器 人 提供 图 像 形 式 的 360? Xf 
境 场 景 。 这 种 视觉 系统 通常 采用 鱼 眼 摄像 头 和 全 景 摄像 头 。 优 点 是 可 处 理 全 视 
场 图 像 ， 但 随 之 而 产生 的 缺点 是 由 于 所 选 摄像 头 特定 类 型 的 几何 尺寸 ， 导 致 成 
本 和 视觉 算法 开发 复杂 性 都 比较 高 。 目 前 已 具有 一 些 基 于 全 方位 视觉 的 导航 系 
统 '“” ， 其 中 大 多 数 都 是 采用 基于 光 流 的 导航 技术 。 

光 流 是 指 对 场景 表面 相对 于 摄像 头 的 运动 而 引发 的 视觉 运动 的 一 种 测量 。 从 
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计算 角度 而 言 ， 最 常见 的 光 流 表示 是 图 像 空间 的 2D 矢量 场 ， 其 中 每 个 矢量 都 描述 
了 一 幅 图 像 中 某 点 到 下 一 幅 图 像 中 相应 位 置 的 运动 。 在 一 个 视觉 导航 的 机 器 人 中 ， 
光 流 很 大 程度 上 是 由 机 器 人 运动 而 产生 的 摄像 头 运动 来 推算 的 。 对 于 光 流 法 ， 许 
多 人 研究 工作 都 是 基于 如 前 所 述 的 蜜蜂 飞行 时 观察 所 采用 的 路 径 中 心 跟踪 方法 。 据 
观察 ， 蜜 蜂 的 飞行 方向 和 速度 都 与 其 相对 于 环境 运动 所 导致 的 视觉 运动 直接 相 
KIO 。 基 于 光 流 ， 移 动机 器 人 在 走廊 中 的 导航 就 是 通过 在 不 同位 置 放 管 的 不 同 
类 型 摄像 头 来 实现 的 。 例 如 ， 在 参考 文献 [68] 中 ， 采 用 了 一 个 前 向 主动 的 广角 摄 
像 头 来 实现 走廊 中 心 定 位 。 在 此 ， 利 用 图 像 左右 2/3 边缘 来 计算 光 流 。 通 过 在 左 
右 2/3 边缘 处 维持 最 大 光 流 ， 使 得 机 器 人 位 于 墙壁 和 障 但 物 之 间 的 中 心路 径 上 。 
在 此 ， 利 用 摄像 头 的 观测 方向 来 计算 机 器 人 在 朝向 调整 过 程 中 的 旋转 角度 。 在 参 
考 文献 [69，70] 中 ， 采 用 不 同 的 方法 来 计算 从 位 于 机 器 人 两 侧 朝向 90° 的 两 个 摄像 
头 得 到 的 平均 横向 光 流 之 差 。 与 参考 文献 [68 不同 ， 这 些 方 法 对 旋转 光 流 没有 进 
行 补偿 。 实 际 上 ， 对 转向 的 限制 可 使 得 所 产生 的 旋转 效果 最 小 。 为 应 对 缺乏 纹理 
的 情况 (如 门口 或 没有 墙壁 ) ， 采 用 能 够 与 一 侧 墙壁 之 间 保 持 恒定 距离 的 单 侧 保持 
行为 ， 这 样 仍 能 够 提供 足够 的 纹理 来 估计 光 流 。 

采用 基于 相关 技术 的 光 流 法 在 本 质 上 类 似 于 立体 视觉 中 的 视差 匹配 方法 。 
不 同 之 处 在 于 光 流 法 中 图 像 是 时 间 上 独立 的 ， 而 立体 视觉 中 图 像 是 空间 独立 的 。 
但 遗憾 的 是 ， 光 流 法 中 图 像 关 联 的 任务 会 由 于 机 器 人 在 真实 环境 中 可 能 受到 振 
动 而 变 得 十 分 复杂 。 这 意味 着 与 立体 视觉 不 同 ， 不 能 只 局 限于 在 同一 水 平 扫 描 
线 上 搜索 相应 图 像 块 ””。 另 外 ， 还 提出 了 采用 基于 梯度 的 方法 来 实现 机 器 人 
连续 运动 下 导航 的 其 他 光 流 技术 “”。 

由 于 光 流 提供 了 在 单 幅 低 质量 图 像 中 无 法 细致 提取 的 场景 质量 特征 ， 目 前 
已 证 明基 于 光 流 的 视觉 导航 技术 对 于 无 人 机 特别 有 效 。 在 该 研究 架构 下 ， 针 对 
视 在 运动 的 处 理 和 应 用 ,已 在 仿生 行为 方面 进行 了 大 量 研究 工作 。 无 人 机 具有 
一 个 由 感光 阵列 构成 的 摄像 头 眼 ， 其 中 每 个 感光 器 都 与 一 个 电子 基本 运动 检测 
器 (EMD ) 相 连 ， 该 EMD 可 计算 特定 位 置 上 的 局 部 光 流 ”。 与 确定 是 否 存在 障 
得 物 的 光 流 计算 相 比 ， 这 种 产生 光 流 测量 变化 的 EMD 极 坐标 确定 可 构建 一 个 包 
含 障 碍 物 位 置 的 局 部 地 图 。 在 参考 文献 [77] 中 ， 也 成 功 实现 了 一 个 具有 由 感光 
阵列 及 其 相应 EMD 组 成 的 摄像 头 眼 的 无 人 机 。EMD 所 获得 的 信息 可 用 于 确定 是 
否 存在 障碍 物 。 另 外 ， 当 无 人 机 以 恒定 的 速度 和 高 度 飞 行 时 ， 可 根据 人 工 视 网 
膜 速率 建 模 方程 来 计算 一 个 参考 光 流 分 布 。 随 着 地 形 的 变化 ， 系 统 可 通过 改变 
推进 力 和 飞行 舵 位 置 来 根据 参考 光 流 调整 在 线 计 算 的 光 流 。 

巡 线 导航 是 另 一 种 在 工业 生产 中 广泛 应 用 的 基于 路 标的 导航 。 巡 线 导航 可 
看 作对 一 个 路 标的 连续 跟踪 ， 尽 管 大 多 数 情况 下 系统 所 用 传感器 要 求 距 离 直线 
非常 近 ， 从 而 导致 机 器 人 的 运动 范围 只 能 限于 直线 附近 。 这 些 技术 常用 于 工业 
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的 自动 化 生产 ， 通 常 将 采用 这 些 技 术 的 车 辆 称 为 AGV (自动 导 引 车 )。 一 辆 AGV 
能 够 在 如 工厂 、 医 院 和 办 公 楼 等 单一 场景 中 的 预 设 路 径 上 导航 ”。 早 期 的 巡 线 
导航 大 多 选择 电缆 磁带 导 引 ， 但 该 方法 的 主要 缺点 是 成 本 高 且 难 以 通过 直线 识 
别 来 重新 定位 导航 路 径 。 基 于 视觉 巡 线 导航 的 早期 成 功 方法 之 一 是 通过 在 采集 
图 像 的 背景 中 提取 一 条 和 白 线 的 图 像 处 理 ， 并 且 该 白 线 会 随 着 车 辆 运动 而 变化 ” 
在 这 项 工作 中 ， 利 用 一 个 TV 摄像 头 来 获取 环境 信息 。 车 辆 的 位 置 是 由 相关 联 的 
场 模式 变化 来 确定 的 ， 这 时 对 机 器 人 电动 机 提供 相应 的 控制 命令 以 使 之 沿路 径 
规划 器 中 预 设 的 路 径 运 动 。 在 参考 文献 [80，81] 中 也 提出 了 类 似 的 基于 预存 路 
标 (信号 和 直线 ) 的 车 辆 导航 。 

另 一 个 引起 广泛 兴趣 的 研究 领域 是 同时 定位 和 地 图 建立 (SLAM ) ， 也 称 为 同 
时 地 图 构建 和 定位 (CML) ， 在 此 移动 机 器 人 能 够 建立 环境 地 图 并 同时 根据 该 地 
图 来 推导 自身 位 置 。 初 始 时 ， 地 图 和 车 辆 位 置 均 未 知 ， 已 知 车 辆 的 动力 学 模型 ， 
并 在 一 个 存在 大 量 人 工 路 标 或 自然 路 标的 未 知 环境 中 运动 。 根据 对 路 标的 观测 ， 
同时 估计 机 器 人 和 路 标的 位 置 。SLAM 问题 中 涉及 对 观测 模型 和 运动 模型 的 合理 
表征 ”。 大 多 数 SLAM 方法 都 是 针对 完全 结构 化 的 静态 环境 下 的 室内 导航 “”， 
并 提供 移动 机 器 人 位 置 和 路 标 位 置 的 相关 度量 信息 。 另外， 还 有 少数 一 些 研 究 
工作 主要 是 针对 动态 场景 和 室外 环境 的 。 在 早期 采用 声 纳 和 视觉 传感器 对 
较 大 实验 区 域 构建 地 图 的 算法 中 ， 尤 为 值得 注意 的 是 对 特殊 (如 长 且 直 的 墙壁 ) 
情况 的 信息 存储 问题 。 随 着 移动 机 器 人 的 运动 ， 由 于 车 轮 打滑 、 地 面 不 平整 以 
及 时 钟 信息 校准 不 佳 而 产生 的 里 程 计 信息 误差 会 导致 里 程 计 提供 的 位 置信 息 越 
来 越 偏离 其 实际 位 置 。 在 这 些 位置 上 检测 到 的 特征 将 内 置 在 与 机 器 人 位 置 相关 
的 地 图 中 ， 因 此 ， 特 征 位 置 也 会 逐渐 偏离 其 实际 位 置 。 现 已 提出 一 些 修 正 上 述 
运动 偏离 的 算法 ， 如 参考 文献 [90，91] 中 提出 保存 每 个 特征 和 机 器 人 位 置 之 间 
的 相关 性 ， 在 参考 文献 [90] 中 提出 采用 扩展 卡尔 曼 滤波 (EKF ) 方 法 来 实现 基于 
视觉 的 SLAM。 尽 管 EKF 方法 已 普遍 应 用 于 解决 这 些 问题 ， 但 都 是 基于 传感器 
和 处 理 过 程 的 不 确定 性 可 建 模 为 高 斯 分 布 的 基本 假设 。 然 而 ,实际 系统 与 这 些 
假设 条 件 存 在 显著 不 同 。EKF 实现 和 KF 实现 的 一 个 主要 缺点 是 对 于 运行 时 间 较 
长 的 任务 ， 路 标 数 会 不 断 增 大 ， 最 终 导致 计算 资源 不 足以 实时 更 新 地 图 。 该 问 
题 的 产生 是 由 于 每 个 路 标 都 与 其 他 路 标 相关 联 。 压 缩 扩展 卡尔 曼 滤 波 器 (CEKF ) 
算法 ”可 在 不 影响 结果 准确 度 的 条 件 下 显著 减少 计算 需求 。CEKF 算法 存储 和 
保持 一 个 局 部 区 域 所 采集 的 所 有 信息 ， 以 及 与 该 区 域 中 路 标 数 二 次 方 成 正比 的 
成 本 ， 然 后 以 类 似 于 完全 SLAM 的 成 本 将 这 些 信息 转移 到 全 局 地 图 中 的 其 余部 
4. 但 只 需 一 次 近代 运行 。 为 解决 上 述 问题 ,针对 基于 视觉 的 SLAM 问题 ， 最 
近 提 出 了 一 些 直接 应 用 粒子 滤波 的 方法 ”” 。 然 而 ， 粒 子 滤波 本 质 上 是 一 个 组 
慢 的 处 理 过 程 ， 因 此 对 于 实时 导航 会 存在 很 大 问题 . 
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1.5 障碍 物 检测 和 避 障 


避 障 是 大 多 数 移 动机 器 人 导航 机 制 中 一 个 十 分 重要 的 步骤。 障碍 物 检测 是 区 分 
地 面 (也 称 为 地 平面 ) 和 地 面 上 物体 的 过 程 ， 即 在 摄像 头 采 集 图 像 中 从 障碍 物 像素 中 
分 离 出 地 面 像素 。 防 撞 是 一 个 使 机 器 人 漫游 旦 避免 与 障碍 物 碰撞 的 转向 行为 。 由 于 
本 书 主要 是 针对 基于 视觉 的 导航 ， 因 此 在 此 仅 限 于 讨论 基于 计算 机 视觉 转向 决策 的 
研究 工作 。 之 前 介绍 的 许多 研究 工作 在 其 他 传统 传感器 的 支持 下 已 实现 避 障 功能 ， 
另外 还 有 一 些 利用 单 日 图 像 特征 、 立 体 视觉 检测 、 光 流 法 和 基于 视觉 的 势 场 法 等 方 
法 来 实现 避 障 。 在 参考 文献 [94] 中 提出 了 一 种 基于 单 目 摄像 头 图 像 的 移动 机 器 人 自 
主 障 碍 物 检测 方法 。 该 系统 主要 包括 三 个 视觉 模块 来 检测 障碍 物 。 这 三 个 视觉 处 理 
模块 分 别 是 基于 亮度 梯度 、RGB 和 HSV， 并 产生 一 个 称 为 障碍 物 边界 的 基于 图 像 的 
粗略 表示 。 这 三 个 模块 的 输出 整合 成 一 个 障碍 物 边 界 ， 并 利用 该 信息 来 产生 转弯 命 
令 。 利 用 三 个 模块 的 目的 是 ， 在 任何 情况 下 ， 都 有 两 个 模块 能 够 适合 于 检测 边界 。 
这 种 方法 已 经 在 JPL( 发 射 推进 实验 室 ) 的 两 种 火星 模拟 环境 中 进行 了 测试 。 然 而 ， 该 
系统 的 缺点 是 当 障 碍 物 超出 摄像 头 视 场 范围 时 就 会 失效 。 

另 一 种 避 障 策略 是 针对 结构 化 环境 的 基于 外 观 的 方法 '”“ 。 该 系统 基于 三 个 
假设 : 在 外 观 上 ， 障 碍 物 与 地 面 不 同 ; 地 面相 对 平坦 ; 所 有 障碍 物 都 与 地 面 接 
触 。 系 统 采用 320 x260 的 彩色 像素 图 像 。 处 理 过 程 主要 包括 四 个 步骤 : 

1) 利用 一 个 5 x5 的 高 斯 掩 膜 来 滤波 彩色 图 像 ; 

2) 将 滤波 后 的 彩色 图 像 转换 到 HIS 彩色 空间 ; 

3) 对 梯形 区 域 中 的 像素 进行 色 度 和 亮度 直方 图 化 ; 

4) 对 滤波 图 像 中 的 所 有 像素 都 进行 色 度 和 亮度 直方 图 对 比 。 如 果 像 素 色 度 
和 亮度 的 直方 图 值 小 于 阔 值 ， 则 将 其 看 作 障 碍 物 。 

该 系统 有 三 种 操作 模式 : 常规 、 自 适应 和 辅助 。 每 种 模式 都 特别 适用 于 一 
种 特殊 情况 。 在 自 适应 模式 中 ， 系 统 可 应 对 光照 变化 ; 在 辅助 模式 中 ， 机 器 人 
可 进行 远程 操作 . 

在 参考 文献 [96] 中 提出 了 一 种 利用 可 视 化 声 纳 进 行 障碍 物 检 测 和 避 障 的 方 
法 。 在 该 方法 中 ， 在 机 器 人 上 安装 一 个 摄像 头 ， 根 据 颜 色 可 将 摄像 头 图 像 中 的 
每 个 像素 分 为 地 面 像素 、 其 他 已 知 物体 或 未 知 物体 。 相 对 于 机 器 人 ， 以 线性 间 
隔 5° 对 该 图 像 进行 扫描 。 如 果 在 对 应 于 同一 类 颜色 的 一 次 扫描 中 存在 一 组 连续 
像素 ， 则 可 将 其 确定 为 一 个 物体 。 当 未 知 颜色 类 同时 出 现时 ， 则 检测 为 未 知 障 
碍 物 。 通 过 物体 和 地 面 最 近 相 交 像 素 点 的 颜色 分 类 值 之 差 来 计算 机 器 人 与 图 像 
之 间 的 距离 ， 利 用 机 器 人 与 物体 之 间 的 距离 和 摄像 头 的 有 限 视 场 来 创建 一 个 局 
部 地 图 。 每 当 在 摄像 头 的 视 场 中 出 现 一 个 新 的 物体 ， 则 更 新 视觉 信息 。 该 视觉 
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算法 已 应 用 于 AIBO 机 器 人 。 在 参考 文献 [97] 中 提出 了 另 一 种 摄像 头 和 声 纳 传 感 
器 相 结合 的 障碍 物 检测 和 避 障 方法 。 在 该 研究 工作 中 ， 利 用 视觉 处 理 中 的 Canny 
边缘 检测 法 来 检测 障碍 物 ， 并 采用 极限 环 和 接近 图 导航 方法 来 实现 避 障 。 

在 基于 立体 视觉 的 障碍 物 检测 方法 中 ， 基 本 思想 都 是 同时 采集 环境 的 两 幅 
图 像 。 障 得 物 的 位 置 可 通过 逆 投 影 变 换 来 确定 ” 。 在 逆 投 影 变换 中 ， 两 个 图 像 
中 的 像素 都 投影 到 地 平面 ， 表 示 地 面 上 的 所 有 点 。 通 过 两 幅 图 像 的 不 同 来 计算 
障碍 物 的 位 置 ， 这 是 因为 这 些 不 同 表明 存在 障 但 物 。 这 种 方法 的 缺点 是 计算 量 
大 。 在 参考 文献 [99] 中 也 提出 了 室外 环境 下 的 障碍 物 检 测 和 避 障 方法 ， 这 是 通 
过 计算 一 对 标定 的 立体 摄像 头 所 采集 两 幅 图 像 的 视差 。 在 该 研究 工作 中 ， 系 统 
假设 物体 较 高 且 表 面 平坦 ， 在 亮度 图 像 中 与 背景 区 别 明显 。 高 于 地 面 的 每 个 点 
都 被 配置 为 一 个 可 能 物体 ， 并 投影 在 地 平面 中 一 个 称 为 瞬时 障碍 地 图 (IOM ) 的 
局 部 占据 栅 格 。 根 据 在 该 瞬时 障碍 地 图 中 计算 的 障 得 物 位 置 来 产生 控制 机 器 人 
转向 的 命令 。 


16 本 章 小 结 


本 章 介 绍 了 基于 视觉 的 自主 移动 机 器 人 导航 的 基本 概念 。 深 入 讨论 了 几 类 
不 同 的 视觉 导航 方法 ， 并 对 现今 该 领域 内 世界 范围 的 研究 工作 进行 了 总 结 。 
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第 2 章 移动 机 器 人 的 外 设 接口 ” 


摘要 : 本 章 介绍 了 如 何在 现 有 移动 机 器 人 上 实现 与 外 设 的 实际 接口 。 由 于 
经 费 限 制 ， 很 多 情况 下 都 无 法 购置 一 个 具有 所 有 外 设 和 传感器 系统 的 完整 机 器 
人 ， 因 此 希望 该 系统 在 这 种 研究 条 件 下 有 很 大 帮助 . 本 章 讨 论 了 如 何 利 用 串口 
通信 以 中 断 驱 动 模式 实现 与 KOALA 机 器 人 的 外 设 接口 。 


2.1 简介 


如 今 ， 许 多 实际 移动 机 器 人 都 配置 有 一 些 能 够 增强 机 器 人 系统 先进 性 和 灵 
活性 并 有 助 于 整体 性 能 和 智能 化 程度 的 传 感 问 和 其 他 辅助 设备 。 在 许多 情况 下 ， 
要 提高 自动 化 程度 需要 与 一 些 实际 驱动 的 附加 外 设 接 口 。 机 器 人 随时 对 这 些 传 
感 器 和 辅助 设备 发 出 控制 指令 ， 而 这 些 外 围 设备 应 在 满足 系统 实时 性 需求 的 条 
件 下 响应 机 器 人 请 求 。 因 此 ， 利 用 机 器 人 核心 部 件 以 及 其 他 附件 成 功 开发 一 个 
集成 系统 需要 复杂 的 中 断 驱 动 软 件 程序 。 目 前， 已 可 为 世界 范围 从 事 机 器 人 研 
究 的 人 员 提 供 多 种 不 同 自动 化 程度 的 机 器 人 平台 ， 大 部 分 这 些 机 器 人 平台 都 配 
置 有 多 个 传感器 和 辅助 设备 以 及 能 够 实现 内 /外 部 实时 通信 的 必要 软件 。 然 而 ， 
如 果 用 户 想 要 在 这 些 机 器 人 平台 上 增加 一 些 不 是 由 同一 生产 厂商 支持 /制造 的 传 
感 器 和 其 他 辅助 设备 作为 外 设 时 ， 就 不 很 方便 。 通 常情 况 下 没有 移动 机 器 人 外 
设 接 口 装置 的 技术 手册 ， 这 是 因为 机 器 人 生产 广 商 并 没有 提供 这 些 技术 资料 ， 
即便 这 些 附 加 传感器 是 与 机 器 人 套件 一 起 购买 的 。 

近年 来 ， 基 于 PIC 微 控 制 器 的 系统 在 一 些 人 研究 领域 得 到 了 广泛 的 实际 应 用 ， 
如 变 容 二 极 管 的 人 工 神经 网 络 模型 的 硬件 开发 ”、 基 于 Petri 网 的 过 程 和 状态 监 
测 分 布 式 系统 ”、 原 子 碰撞 实验 中 常用 的 双 射 束 调制 系统 的 开发 ”、 递 归 神 经 
网 络 模型 的 硬件 实现 “” 、 模 糊 控制 的 同步 电动 机 的 无 功 功率 控制 ”等 。PIC 微 控 
制 器 的 架构 是 基于 一 种 改进 的 哈佛 RISC 484 $8 ^" 。 由 于 可 提供 出 色 的 低 成 本 
解决 方案 和 先进 性 能 ， 该 技术 越 来 越 得 到 广泛 应 用 。 鉴 于 数据 和 指令 的 传输 





加 ”本 音 基 于 N. Nirmal Singh, Amitava Chatterjee 和 Anjan Rakshit 2010 年 发 表 于 International Journal of 
Electronics 期 刊 第 97 期 ，139 ~ 161 页 的 论文 “A PIC Microcontroller system for Real-life Interfacing of 
External Peripherals with a Mobile Robot”。 经 出 版 商 许 可 后 重印 (Taylor & Francis Ltd, http: // 


www. tandf. co. uk/journals ) 。 
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是 在 独立 总 线 上 实现 的 ， 因 此 能 够 提供 满意 的 性 能 。 这 些 处 理 器 还 能 够 提高 软 
件 代 码 效 率 ， 并 在 取 下 一 条 指令 的 同时 执行 当前 指令 ”。 本 章 将 详细 讨论 如 何 
开发 一 个 基于 PIC 微 控 制 器 的 系统 ， 使 之 能 够 实现 目前 市 场 上 已 有 的 常见 移动 
机 器 人 与 外 设 接口 ，”" 。 在 此 所 采用 的 移动 机 器 人 是 由 瑞士 K-team S. A 公司 生 
产 的 KOALA 机 器 人 。 印 度 加 尔 各 答 茧 达 普 (Jadavpur) 大 学 电子 工程 学 院 电子 测 
量 和 电子 设备 实验 室 购置 的 KOALA 机 器 人 仅 配 置 有 增 量 编码 器 、 声 纳 传 感 器 
红外 传感器 。 但 是 为 了 能 够 利用 视觉 感知 对 KOALA 移动 机 器 人 进行 导航 ， 需 要 
对 机 器 人 开发 并 集成 一 个 视觉 感知 系统 的 接口 装置 。 因 此 ， 最 初 的 研究 工作 是 
直接 在 KOALA 机 器 人 上 分 别 增 加 一 个 立体 视觉 装置 (由 两 个 摄像 头 组 成 ) 和 一 个 
单 目 视觉 装置 (只 由 一 个 摄像 头 组 成 ) ， 这 样 就 使 得 机 器 人 在 导航 时 可 灵活 选择 
双 摄 像 头 系统 或 单 摄像 头 系统 。 本 章 主 要 讨论 在 KOALA 机 器 人 外 部 集成 的 视觉 
系统 中 增加 一 个 四 自由 度 (DOF ) 的 立体 视觉 装置 ， 开 发 一 个 用 于 机 器 人 系统 云 
台 控 制 、 左 右 摄像 头 聚焦 控制 的 PIC 微 控制 器 系统 。 同 时 还 详细 介绍 了 以 中 断 
驱动 模式 开发 的 应 用 软件 ， 有 助 于 其 他 用 户 开 发 类 似 的 集成 系统 。 这 样 可 使 得 
研究 人 员 《/ 开 发 人 员 对 机 器 人 保持 完全 灵活 性 ， 同 时 开发 成 本 也 会 显著 减少 。 事 
实 上 ， 这 项 研究 工作 的 主要 动机 是 由 于 预算 不 足以 购买 集成 了 视觉 系统 的 KOA- 
LA 机 器 人 完整 套件 ， 因 此 只 能 在 实验 室 自主 实现 KOALA 机 器 人 与 视觉 系统 的 
集成 。 在 此 也 真心 希望 这 项 工作 能 够 鼓励 机 器 人 领域 的 其 他 研究 人 员 能 够 自主 
开发 这 种 能 够 使 得 独立 外 设 与 其 他 机 器 人 套件 接口 的 中 断 驱 动 系统 。 这 样 将 有 
助 于 开发 高 复杂 度 的 低 成 本 机 器 人 平台 ,尽管 现 有 系统 仅 具 有 四 个 外 设 ( 即 四 自 
由 度 的 四 个 RC 伺服 电动 机 ) 接口 ， 但 从 人 逻辑 上 可 扩展 更 多 的 外 设 。 





2.2 用 于 视觉 系统 与 现 有 机 器 人 接口 的 PIC 微 控制 器 系统 


本 节 所 开发 的 系统 采用 一 个 PIC 16F876A 微 控制 器 来 实现 KOALA 机 器 人 与 
一 个 视觉 系统 的 实时 接口 ， 目 的 是 为 了 实现 四 个 RC 伺服 电动 机 "与 KOALA 移 
动机 器 人 的 实际 接口 ， 由 此 可 对 集成 于 KOALA 机 器 人 中 的 视觉 系统 增加 四 自由 
度 。 图 2.1 给 出 本 实验 室 开发 的 集成 了 视觉 系统 的 KOALA 机 器 人 完整 系统 。 其 
中 ,包括 内 置 有 红外 、 增 量 编 码 器 等 传感器 的 基本 KOALA 机 器 人 。 为 提高 机 器 
人 系统 的 能 力 ， 另 外 还 集成 了 两 个 声 纳 传感器 和 两 个 摄像 头 来 实现 立体 视觉 功 
能 。 然 而 ， 如 前 所 述 ， 在 KOALA 机 器 人 中 开发 的 视觉 导航 系统 还 配置 了 独立 的 
单 目 视觉 功能 。 在 利用 立体 视觉 开发 的 系统 和 利用 单 目 视觉 开发 的 系统 之 间 只 
有 一 个 显著 差别 。 单 目 视 觉 情况 下 ， 在 主动 式 云 台 系 统 的 中 心 只 安装 一 个 摄像 
头 ， 且 系统 只 利用 两 个 自由 度 来 实现 云 台 控 制 。 所 开发 的 集成 视觉 系统 应 能 够 
实现 对 于 立体 视觉 四 自由 度 和 对 于 单 目 视觉 两 自由 度 的 灵活 控制 。 因 此 ， 为 提 
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高 视觉 系统 的 高 度 灵 活性 ， 一 个 云 台 系统 集成 了 四 个 伺服 电动 机 。 图 2.2 给 出 
用 于 驱动 四 个 伺服 电动 机 的 PIC 微 控制 器 的 原理 示意 图 ”. 





图 2.1 KOALA 机 器 人 的 完整 视觉 系统 
+5V 







+5V 





0.1uF 电容 


R 伺服 电动 机 4 


select 
clock 


data 


R 伺服 电动 机 3 


20MHz "ES 


晶振 R 伺服 电动 机 2 


R 伺服 电动 机 1 


图 2.2 用 于 KOALA 机 器 人 与 四 个 伺服 电动 机 接口 的 PIC 16F876A 控制 板 的 原理 示意 图 
(Y: 黄色 ; R: 红色 ; B. 黑色 ) 
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利用 四 个 RC 伺服 电动 机 来 实现 完整 视觉 系统 的 云 台 控 制 ， 以 及 左右 摄像 头 
的 各 自 聚 焦 控 制 。PIC 16F876A 微 控 制 器 接收 来 自 于 Motorola 68331 处 理 器 中 三 
个 输入 引 脚 [select( 片 选 信号 ) n clock (时 钟 信号 ) 和 data( 数 据 信 叶 )] 的 信和 号。 
68331 处 理 器 工作 在 SPI 主 模式 ， 而 PIC 16F876A 微 控制 器 工作 在 SPI 从 模式 。 
图 2.3 给 出 了 所 用 PIC 微 控 制 闫 板 的 实物 图 片 。 该 控制 器 板 安装 在 与 云 台 系 统 文 
架 相 垂 直 的 位 置 上 ， 使 得 整个 集成 系统 的 结构 足够 坚固 











图 2.3 所 用 PIC 微 控制 器 板 的 实物 图 片 

如 前 所 述 ， 该 系统 采用 了 一 个 28 引 脚 双 列 直 择 式 封 装 (PDIP) 的 PIC 
16F876A 微 控 制 器 。PIC 16F876A 微 控制 器 的 主要 特点 是 ”包含 8KB 的 flash 程 
序 存储 器 、368B 的 数据 存储 器 和 256B 的 EEPROM 数据 存储 器 。 工 作 频 率 为 
0Hz ~20MHz， 并 提供 14 种 中 断 。PIC 16F876A 微 控制 器 具有 三 个 1/0 接口 (A、 
B 和 C)、 三 个 定时 器 、 两 个 模拟 比较 器 和 五 个 10bit A-D 转换 输入 通道 。 通 过 
MSSP 和 USART 实现 串口 通信 。 图 2.4 给 出 了 28 引 脚 双 列 直 插 式 封 装 PIC 
16F876A (I T il as H3 5| BA EAT 

算法 2.1 描述 了 实现 KOALA 机 器 人 与 外 设 实 际 接口 操作 (本 例 中 为 视觉 系 
统 的 RC 伺服 电动 机 ) 的 PIC 微 控制 器 主 程序 算法 . 其中， 介绍 了 如 何 对 PORTB 
的 数据 方向 寄存 器 A-D 转换 控制 寄存 器 、MSSP 控制 寄存 器 、INTCON 寄存 器 、 
PIE1 寄存 器 和 PIRI 寄存 器 进行 编程 以 及 具体 的 编程 顺序 。 不 断 对 系统 初始 化 ， 
使 得 在 每 个 RC 伺服 电动 机 的 中 间 位 置 初 始 化 并 等 待 一 个 合适 的 输入 驱动 指令 。 
然后 ， 系 统 允许 同步 串口 中 断 (SP1)， 而 对 于 PIC 微 控制 占 ， 在 从 模式 下 设置 
SPI, HERR, 根据 中 断 服务 标志 位 ， 由 具体 指令 驱动 相应 的 RC 伺服 电动 机 。 
然后 ， 复 位 中 断 服务 标志 位 ， 以 使 下 次 新 的 中 断 请 求 发 生 时 可 进行 设置 。KOA- 
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MCLR/VPP RB7/PGD 


RAO/ANO RB6/PGC 


RAIANI RBS 


RA2/AN2/VRFF-/CVRFF RB4 
RA3/AN3/VRFF+ RB3/PGM 
RA4/TOCKI/C1OUT RB2 


RAS/AN4/SS/C20UT RBI 


< 
o 
e 
oo 
a 
Nol 
eo 
= 


VSS RBO/INT 


OSCI/CLKI VDD 


OSC2/CLKO VSS 


RCO/TIOSO/TICKI RC7/RX/DT 


RCI/TIOSI/CCP2 RC6/TX/CK 


RC2/CCPI RCS/SDO 





RC3/SCK/SCL RC4/SDI/SDA 
图 2.4 28 引 脚 双 列 直 插 式 封 装 PIC 16F876A 微 控制 器 的 引 脚 图 

LA 机 器 人 的 主要 技术 特点 ”是 包括 一 个 工作 频率 为 22MHz 的 Motorola 68331 处 
Bilge; Blase AMY RAM 容量 为 1MB， 且 内存 容量 也 是 1MB， 但 没有 ROM; 为 实 
现 运动 ，KOALA 机 器 人 配置 了 具有 增 量 编码 固 的 直流 电动 机 ; 与 车 轮 相 连 的 直 
流 电动 机 通过 一 个 58.5:1 的 减速 齿轮 来 实现 车 轮 运动 KOALA 机 器 人 的 主 处 理 
器 具有 直接 控制 电动 机 供电 电源 的 装置 ; 该 处 理 咒 可 读 取 增 量 编码 器 的 脉冲 信 
号 ; RS232 串口 通信 和 总 是 设置 为 包括 一 个 起 始 位 、 两 个 停止 位 和 无 奇偶 校 验 的 
8bit 模式 ; 波 特 率 变化 范围 为 9600 ~ 115200; 机 器 人 具有 十 二 个 数字 输入 接口 、 
四 个 COMS/TTL 数字 输出 接口 、 八 个 电源 数字 输出 接口 和 六 个 模拟 输入 接口 ; 
机 融 人 本 体 配 置 有 十 六 个 红外 接近 传感器 。 这 些 红 外 传感器 中 包括 一 个 红外 
LED 和 一 个 接收 器 ， 由 德州 仪器 公司 制造 (TSL252 型 ) ， 可 用 于 环境 光 测量 和 反 
射 光 测量 ， 其 输出 值 是 一 个 由 10bit A-D 转换 器 转换 的 模拟 量 。 因 此 ， 在 KOALA 
机 项 人 的 基本 架 侈 上 ， 集 成 了 一 个 由 双 目 摄像 头 、 云 台 系 统 、 四 个 RC 伺服 电动 
机 和 PIC 微 控 制 器 板 构成 的 视觉 系统 ， 从 而 构成 了 一 个 改进 型 机 器 人 。 

Motorola MC68331 处 理 器 是 一 个 32bit 微 控 制 器 ， 具 有 与 外 设 之 间 的 高 速 数 
据 处 理 能 力 。 该 微 控制 器 包括 一 个 32bit CPU 、 一 个 系统 集成 模块 、 一 个 通用 计 
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时 器 (CPT) 和 一 个 队列 串 行 模块 (QSM ) 。MC68331 控制 单元 的 一 个 主要 优点 是 
低 功 耗 。CPU 基于 工业 级 的 MC68000 处 理 器 ， 综 合 了 MC68010 和 MC68020 处 
理 器 的 许多 特征 ， 并 增加 了 高 性 能 控制 器 应 用 的 特殊 功能 。 通 过 采用 11 通道 的 
GPT 可 实现 中 等 规模 的 CPU 控制 。 这 些 GPT 引 脚 还 可 配置 为 通用 的 1/0 接口 。 
QSM 由 两 个 串 行 接口 组 成 : 队列 串 行 外 设 接口 (4QSPI); 串 行 通信 接口 (SCI) 。 
QSPI 可 提供 简单 的 外 设 扩 展 或 处 理 需 间 通 信 。 

算法 2.1 用 于 外 设 实际 接口 的 PIC 微 控制 器 主 程序 算 法 





1. 初始 化 PORT B, 
2. 设置 对 应 于 PORT B 的 数据 方向 寄存 器 ， 使 得 PORT B 的 4-7 引 脚 配置 为 
数字 输出 接口 。 
3. 设置 A-D 转换 控制 寄存 器 的 内 容 为 1， 使 得 A-D 转换 接口 的 所 有 引 脚 都 
配置 为 数字 IO 接口 。 
4. 设置 角 位 置 指令 ， 使 得 每 个 RC 伺服 电动 机 都 位 于 中 间 位 置 ， 即 0° 位 置 。 
5. 设置 主 同步 串口 (MSSP) 控制 寄存 器 为 1( 在 SPI 模式 下 )， 使 得 同步 串口 
使 能 ， 并 且 SCK, SDO, SDI 和 SS 配置 为 串口 引 脚 ， SPI 设置 为 从 模式 ， 且 SS 
引 脚 控制 使 能 。 
6. 设置 MSSP 状态 寄存 器 (在 SPI 模式 下 ) ， 使 得 重 置 SPI 时 钟 信号 选择 位 。 
7. 设置 外 设 中 断 使 能 寄存 器 为 1， 使 得 同步 串口 (SSP) 中 断 使 能 。 
8. 设置 INTCON 寄存 器 ， 使 能 所 有 未 屏蔽 外 设 中 断 和 全 局 未 屏蔽 中 断 。 
9. HH PIRI 寄存 器 的 SSP 中 断 标志 位 来 表明 初始 时 不 会 发 生 SSP 中 断 。 
10. IF 设置 中 断 服务 标志 位 
THEN 
检查 RC 伺服 电动 机 的 ID ， 以 确定 产生 何 种 中 断 。 
综合 八 位 寄存 器 值 、 高 字 节 数据 和 低 字 节 数 据 ,， 来 产生 该 伺服 电 
动机 的 16 位 角 位 置 指令 。 
重 置 中 断 服务 标志 位 。 
ENDIF 
11. 设 j=1。 
12. FOR j=1 to 4, 
使 得 对 RC 伺服 电动 机 (j) 产 生 驱 动 指令 的 PORT B 的 RBx 引 脚 为 高 电 平 。 
保持 RBx 一 段 时 间 内 为 高 电 平 ， 作 为 其 相应 角 位 置 指令 的 函数 进行 计算 。 
之 后 将 RBx 设置 为 低 电 平 。 
ENDFOR 
13. 保持 每 个 RB4 ~ RB7 引 脚 为 低 电 平 20ms。 
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14. 清空 CPU 的 看 门 狗 。 
15. 返回 至 第 10 步 。 





算法 2.1 与 同步 串口 中 断 子 程序 的 算法 2.2 共同 运行 。 这 是 在 实际 外 设 应 用 
中 所 执行 的 一 个 非常 复杂 的 过 程 。 在 此 ， 算 法 2.2 阐述 了 如 何 读 取 输 入 驱动 命 
令 来 执行 特定 的 驱动 命令 。 角 位 置 命令 为 2B， 且 高 字 节 和 低 字 节 均 以 串 行 方式 
传输 。 实 际 上 ， 发 送 的 第 一 个 信息 是 驱动 所 需 的 RC 伺服 电动 机 ID ， 接 下 来 是 
串 行 的 高 字 节 和 低 字 节 的 位 置 命令 ， 用 于 指定 RC 伺服 电动 机 应 旋转 多 少 度 。 图 
2.2 给 出 了 外 部 时 钟 频 率 为 20MHz 的 PIC 16F876A 微 控制 器 ， 这 是 PIC 16F876A 
处 理 器 的 最 大 允许 工作 频率 ， 且 最 高 允许 值 用 于 实现 该 特定 应 用 的 最 佳 性 能 。 
PIC 处 理 器 使 用 三 个 输入 信号 : select( 用 于 触发 ) 、clock( 时 钟 信号 ) 和 data ( 确定 
电动 机 驱动 及 其 角 位 置 的 输入 波形 数据 ) 。PIC 处 理 器 的 输出 取 为 PORTB 的 四 个 
引 脚 ， 分 别 为 每 个 RC 伺服 电动 机 产生 各 自 的 角 位 置 命令 。 

算法 2.2 同步 串 行 中 断 服务 子 程序 算法 





1. IF count_flag =0 
THEN 
读 取 SSP 接收 /发 送 缓存 寄存 器 的 值 作 为 RC 伺服 电动 机 ID 。 
ENDIF 
2. IF count. flag = 1 
THEN 
BOR SSP 接收 /发 送 缓存 寄存 器 的 值 作为 “高 字 节 数据 "， 即 角 位 置 命令 
的 高 字 节 。 
ENDIF 
3. IF count. flag =2 
THEN 
读 取 SSP 接收 /发 送 缓存 寄存 器 的 值 作 为 “ 低 字 节 数 据 ”， 即 角 位 置 命令 
的 低 字 节 。 
ENDIF 
4. count_flag 加 1。 
5. IF count_flag >2 
THEN 
重 置 count_flag X 0, 
设置 中 断 服 务 标志 位 。 
ENDIF 
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6. EA PIRI 寄存 器 的 SSP 中 断 标志 位 。 





在 算法 2. 1 中 ， 步 又 1 ~9 用 于 初始 化 。 在 开始 时 ， 对 8 位 双向 接口 的 
PORTB 初始 化 。 PORTB 对 应 的 数据 方向 寄存 器 记 为 TRISB. 通过 合理 设置 
TRISB 寄存 器 ， 可 为 数字 输入 或 数字 输出 设置 每 个 相应 的 PORTB 引 脚 。 在 本 实 
验 中 ， 对 PORTB 编程 实现 其 高 四 位 引 脚 ， 即 引 脚 4 ~7( 记 为 RB4 ~ RB7 ) 配 置 为 
数字 输出 接口 ， 用 于 产生 四 个 伺服 电动 机 的 角 位 置 命令 。 

ee pe 对 于 PIC 16F876A 微 控制 
器 ， 该 模块 具有 五 个 输入 接口 。 在 此 ， 对 于 某 一 给 定 模拟 输入 信号 ， 该 模块 产 
生 一 个 10 位 数字 作为 输出 2d su 器 : A-D 转换 结果 高 
位 寄存 器 (ADRESH) ; A-D 转换 结果 低位 寄存 器 (ADRESL) ; A-D 转换 控制 寄存 
$& O( ADCONO) ; A-D 转换 控制 寄存 器 1(ADCON1 )。ADCON1 寄存 器 可 用 来 设 
置 接口 引 脚 为 模拟 输入 接口 或 数字 LO 接口 。 图 2.5 给 出 对 ADCONI 寄存 器 的 
描述 “ 。 对 于 所 开发 的 系统 ， 对 A-D 转换 接口 设置 控制 位 编程 ， 使 得 这 些 引 脚 
均 设置 为 数字 1/0 接口 。 

在 PIC 16F876A 微 控制 器 中 ， 与 其 他 外 设 或 微 控制 器 设备 的 串口 通信 由 主 
同步 串口 (MSSP) 模 块 控制 实现 。MSSP 模块 是 一 个 可 与 EEPROM A-D 转换 寄 
存 器 、 移 位 寄存 器 等 进行 通信 的 串 行 接口 。MSSP 模块 可 工作 于 串 行 外 设 接口 
(SPI) 模式 或 内 置 集成 电路 (EC ) 模式 。 在 本 实验 中 ,在 SPI 模式 下 进行 系统 开 
发 ， 这 时 以 一 种 同步 方式 同时 发 送 和 接收 八 位 数据 。 在 SPI 工作 模式 下 ，MSSP 
模块 中 使 用 了 一 个 状态 寄存 器 (SSPSTAT) 和 两 个 控制 寄存 器 (SSPCON 和 
SSPCON2) 。 另 外 ， 还 有 两 个 其 他 寄存 器 ， 即 串 行 接收 /发 送 缓存 寄存 器 (SSP- 
BUF) 和 MSSP 移 位 寄存 器 (SSPSR) 。 在 这 些 寄存 器 中 ，SSPSR 寄存 器 不 能 直接 
访问 ， 只 能 通过 定位 SSPBUF 寄存 器 来 访问 。 在 本 系统 中 ，PIC 16F876A 处 理 器 
工作 在 从 模式 ， 而 KOALA 机 器 人 中 的 Motorola 68331 处 理 器 工作 在 主 模式 。 图 
2.6 给 出 了 SSPSTAT 和 SSPCON 寄存 器 的 具体 描述 。 通 常 ， 有 = :个 引 脚 用 于 通 
fi. 串 行 数据 输出 (SDO) ; 申 行 数据 输入 (SDI) ;品行 时 钟 (SCK) 当 工 作 在 
SPT Ashe, SERSA TINERE ESILLE, wf arce 
寄存 器 编程 ， 使 得 SSPEN 位 为 1， 使 能 串口 ， 并 配置 SCK, SDO, SDI 和 SS 引 
脚 为 串口 引 脚 。 同 理 ， 对 SSPM3: SSPMO 位 编程 ， 使 得 PIC 处 理 器 配置 为 SPI 从 
模式 ， 且 将 其 时 钟 信 号 赋予 SCK 引 脚 。 因 此 ， 在 图 2. 2 中 ，select 对 应 于 SS 引 脚 ， 
时 钟 信号 在 SCK 引 脚 ， 以 及 数据 位 于 SDI1 引 脚 。 这 样 就 能 确保 数据 传输 发 生 在 
从 活动 状态 到 空闲 时 钟 状 态 的 转移 过 程 中 。 图 2.7 给 出 了 在 本 系统 中 实现 Mo- 
torola 处 理 器 与 PIC 处 理 器 接口 的 SPI 主 / 从 连接 编程 。 在 接收 数据 时 ， 由 于 PIC 
处 理 器 是 在 从 模式 下 编程 ，SSPSR 和 SSPBUF 寄存 器 共同 创建 一 个 双 缓 存 接 收 
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器 。 在 此 ，SSPSR 移 位 寄存 器 用 于 移 人 或 移出 数据 (MSB 优先 ) ，SSPBUF 寄存 器 
是 数据 字 节 写 人 或 读 出 的 缓存 寄存 器 。 


ADFM | ADCS2 - s ps Rei | PCFGI lid 


bit 7 bit 0 
bt7 ADFM (A-D 转换 结果 格式 选择 位 ) 

bit6 - ADCS2 (A-D 转换 时 钟 选择 位 ) 

bit 5-4 : 保留 位 

bit 3-0 : PCFG3-PCFGO (A-D 转 换 接口 配置 控制 位 ) 











图 2.5 ADCON1 寄存 器 描述 9 


SMP ie all lid [? | | 
bit 7 bit 0 
bit 7: SMP (采样 位 ) 

bit 6: CKE (SPI 时 钟 选择 位 ) 

bit 5: D/A (数据 /地 址 位 ) 

bit4: P (停止 位 ) 

bit3: S (起 始 位 ) 

bit 2: RAW ( 读 / 写 信息 位 ) 

bit1: UA (更 新 地 址 位 ) 

bit0: BF (缓冲 满 状态 位 ) 














(只 使 用 于 接收 模式 ) 
a) 
| WCOL | SSPOV | SSPEN | CKP | SSPM3 | SSPM2 e SSPMO 
bit 7 bit O 
bit 7: WCOL ( 写 冲 突 检 侧 位 ) 
(只 适用 于 发 送 模 式 ) 


bit 6 : SSPOV (接收 溢出 标志 位 ) 

bit 5: SSPEN (同步 串口 使 能 位 ) 

bit 4 : CKP (时 钟 极 性 选择 位 ) 

bit 3 : SSPM3-SSPMO (同步 串口 模式 选择 位 ) 


b) 


E] 2.6 SSPSTAT 和 SSPCON 寄存 器 描述 (SPI o) O 


Motorola 68331 


处 理 器 1 





MSB PIC 16F876A LSB | 
I— — SCK 处 理 器 2 


图 2.7 系统 中 编程 实现 SPI 主 / 从 连接 
接 下 来 ， 置 PIE1 寄存 器 的 SSPIE 位 为 1， 以 使 能 同步 串口 (SSP) 中 断 。PIE1 
寄存 器 中 包括 单独 的 外 设 中 断 使 能 位 。 在 此 应 注意 到 ，INTCON 寄存 器 的 PEIE 
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位 必须 置 为 1 以 使 能 任何 未 屏蔽 外 设 中 断 。 从 用 户 编程 角度 来 看 ，INTCON 寄存 
器 非常 重要 ， 这 是 由 于 该 寄存 絮 中 包含 几 个 TMRO 寄存 器 溢出 、RB 接口 改变 和 
外 部 RBO/INT 引 脚 中 断 的 使 能 位 和 标志 位 。 图 2. 8 分别 给 出 PIE1 寄存 器 和 IN- 
TCON 寄存 器 的 详细 描述 。 在 程序 中 ，INTCON 寄存 器 中 的 PEIE 位 和 GIE 位 均 
设 为 1。 应 注意 到 无 论 GIE 位 及 其 相应 使 能 位 的 状态 如 何 ， 在 中 断 条 件 发 生 时 ， 
都 需 设 置 中 断 标志 位 为 1。 因此 ， 编 写 用 户 程序 时 ， 在 中 断 使 能 之 前 ， 必 须 注 意 
要 重 置 合适 的 中 断 标志 位 。 因 此 ， 在 初始 时 重 置 PIR 寄存 器 的 SSPIF 位 ， 以 表 
明 没 有 发 生 SSP 中 断 条 件 且 系统 已 准备 好 SSP 中 断 使 能 。PIR1 寄存 器 是 一 个 包 
含 外 设 中 断 独 立 标志 位 的 特殊 寄存 器 ， 因 此 编写 用 户 程序 时 ， 在 一 个 特殊 中 断 
使 能 之 前 ， 必 须 重 置 PIR1 寄存 器 中 相应 的 标志 位 。 图 2.9 给 出 了 PIRI 寄存 器 的 
详细 描述 。 


| PSPIE | ADIE | RCIE | TXIE | SSPIE | CCPHE | TMR2IE | TMRIIE | 








bit 7 bit 0 
bit 7 : PSPIE (并 行 从 接口 读 / 写 中 断 使 能 位 ) 

bit 6 : ADIE (A-D 转 换 中 断 使 能 位 ) 

bit 5 :RCIE(USART 接收 中 断 使 能 位 ) 

bit4 :TXIE (USART 发 送 中 断 使 能 位 ) 

bit3 :SSPIE (SSP 中 断 使 能 位 ) 

bit2 : CCPIIE (中 断 使 能 位 ) 

bit 1 : TMR2IE (TMR2 到 PR2 匹 配 中 断 使 能 位 ) 

bit0 : TMRIIE (TMRI1 溢 出 中 断 使 能 位 ) 


a) 


[GE | PEIE | TMROIE [INTE [| RBIE | TMROIF TINTF [RBIF | 
bit7 bit 0 
bit 7 : GIE (全 局 中 断 使 能 位 ) 
bit 6 : PEIE (外 设 中 断 使 能 位 ) 
bit 5: TMROIE (TMR0 溢 出 中 断 使 能 位 ) 
bit 4 : INTE (RBO/INT 外 部 中 断 使 能 位 ) 
bit 3 : RBIE (RB 接口 变化 中 断 使 能 位 ) 
bit 2 : TMROIF (TMRO 溢 出 中 断 标志 位 ) 
bit 1 : INTF (RBO/INT 外 部 中 断 标志 位 ) 
bit 0 : RBIF (RB 接口 变化 中 断 标志 位 ) 


b) 








图 2.8 PIEL 和 INTCON 寄存 器 描述 


PSPIF | ADIF | RCIF | TXIF | SSPIF | CCPIIF | TMR2IF | TMRIIF | 
bit 7 bit 0 
bit 7 : PSPIF (并 行 从 接口 读 / 写 中 断 标志 位 ) 

bit 6 : ADIF (A-D 转 换 中 断 标志 位 ) 

bit 5: RCIF (USART 接 收 中 断 标志 位 ) 

bit 4 : TXIF (USART 发 送 中 断 标志 位 ) 

bit 3 : SSPIF (SSP 中 断 标志 位 ) 

bit 2: CCPIIF (CCP1 中 断 标志 位 ) 

bit 1 : TMR2IF (TMR2 到 PR2 匹 配 中 断 标志 位 ) 

bit 0 : TMRIIF (TMR1 溢 出 中 断 标志 位 ) 


图 2.9 PIRI 寄存 器 描述 
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一 旦 完成 初始 化 ， 算 法 2. 1 所 述 的 主 程序 中 步骤 10 ~ 15 与 算法 2.2 中 的 串 
行 中 断 服务 子 程序 共同 执行 一 个 无 限 循 环 。 在 中 断 子 程序 中 ，PIC 处 理 器 在 从 模 
式 下 等 待 Motorola 处 理 器 写 和 人/ 发送 的 数据 。 图 2. 10 给 出 接收 到 的 SPI 写 数 据 形 
式 。 波 形 表 明 一 个 字 节 数据 的 写 信 ， 对 每 个 写 和 人 的 字 节 ， 即 PIC 处 理 器 接收 的 
每 个 字 节 ， 不 断 重复 执行 该 过 程 。 对 系统 编程 使 得 每 个 数据 位 都 可 在 时 钟 信号 
的 上 升 沿 或 下 降 沿 锁 存 。 在 主 模式 下 ， 数 据 传输 总 是 由 Motorola 处 理 器 通过 发 送 
SCK 信号 进行 初始 化 。PIC 处 理 器 的 SS 引 脚 为 低 电 平 ， 则 使 能 数据 发 送 和 接收 操 
作 。 而 在 从 模式 下 ， 若 SS 引 脚 为 高 电 平 或 通过 清除 SSPEN 位 来 重 置 SPI 模块 。 
在 本 系统 中 ，SPI 模块 是 通过 强制 设置 SS5 引 脚 为 高 电 平 来 重 置 的 。SSPBUF 寄存 
器 保存 将 要 写 入 SSPSR 寄存 器 的 数据 ， 直 到 所 接收 的 数据 准备 好 。 如 前 所 述 ， 
数据 是 按 字 节 一 个 一 个 地 接收 ， 对 于 每 个 接收 的 字 节 ， 一 旦 接收 了 Sbi 数据 ， 
则 将 该 字 节 转 移 到 SSPBUF 寄存 器 。 通 过 设置 缓存 区 满 检 测 位 (BF) 和 中 断 标志 
位 (SSPIE ) 为 1 来 标记 该 操作 。 因 此 ， 这 种 双 缓 存 机 制 能 够 在 全 部 读 入 刚 接收 的 
数据 字 节 之 前 开始 接收 新 数据 字 节 。 由 Motorola 处 理 器 发 送 ， 然 后 由 PIC 处 理 器 
在 该 串 行 中 断 模 式 下 接收 的 数据 字 节 序 列 按 如 下 编程 : 


Select 
(或 SS) | | 
uvm 


(或 SDD 


— | | ee ER 


clock 
(或 SCK) 


图 2. 10 SPI 模式 的 波形 (从 模式 ) 

1) 发 送 包 含 RC 伺服 电动 机 ID(1 ~4) 的 数据 字 节 。 

2) HEIR 10ms。 

3) 对 特定 RC 伺服 电动 机 ID 发 送 相应 角 位 置 命令 的 高 字 

4) HEIR 10ms。 

5) 对 同一 RC 伺服 电动 机 ID 发 送 相应 角 位 置 命令 的 低 字 节 。 

在 1)、3) 和 5) 的 每 个 步骤 中 ，SSPBUF 寄存 器 读 和 不同 的 临时 变量 。 当 完 
成 1) ~5) 步 又 时 ， 也 就 实现 了 一 次 完整 的 信息 传输 周期 。 通 过 在 软件 中 设置 一 
个 临时 标志 变量 (中 断 服务 标志 位 ) 来 进行 标记 。 在 从 中 断 服务 子 程序 返回 之 前 ， 
用 户 程 序 还 必须 重 置 PIRI 寄存 器 中 的 SSP 中 断 标志 位 (SSPIF ) 。 一 旦 在 主 程序 


P 


DG 


d 
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中 设置 了 该 中 断 服 务 标志 位 ， 则 合并 RC 伺服 电动 机 角 位 置 命令 的 高 字 节 和 低 字 
节 来 创建 一 个 合适 的 角 位 置 命令 ， 同 时 对 RC 伺服 电动 机 发 送 一 个 相应 的 驱动 命 
令 。 对 于 每 个 RC 伺服 电动 机 ， 通 过 驱动 相应 的 引 脚 信 号 以 维持 一 段 时 间 的 高 电 
平 ， 然 后 驱动 同一 引 脚 信号 维持 一 段 时 间 的 低 电 平 来 产生 该 数字 驱动 信号 
1. 5ms 的 高 电 平 信号 对 应 于 0? 的 角 位 置 命令 ，1lms 的 高 电 平 信号 则 对 应 - 90°, 
2ms 时 对 应 +90"。 对 于 所 有 中 间 的 角 位 置 命令 ， 发 送 一 个 成 正比 的 高 电 平 信和 号 
维持 时 间 。 高 电 平 信号 之 后 总 是 一 个 20ms 固定 时 间 的 低 电 平 信 叶 。 图 2.1]1 给 
出 了 在 PIC 16F876A 处 理 器 的 RB4 、RB5 、RB6 和 RB7 引 脚 处 产生 的 输出 波形 . 


I NN NO 


>| | -90* 位 置 时 Ims 的 脉 宽 


| | 0° 位 置 时 1 5ms 的 脉 宽 


| oe ge E E 


| k- +90 位 置 时 2ms 的 脉 宽 


图 2.11 PIC 16F876A 处 理 器 在 RB4、RB5、RB6 和 RB7 引 脚 处 产生 的 输出 波形 
Pulse width of Ims for —90 position - 90 位置 时 Ims 的 脉 宽 

如 上 所 述 ， 在 该 具体 实现 中 ， 采 用 了 SPI 模式 而 不 是 下 C 模式 来 进行 串口 通 
言 。 众 所 周知 ，TC 模式 更 适用 于 多 分 支 总 线 架 构 的 设备 ， 而 系统 也 随 着 所 涉及 
的 复杂 性 增加 而 变 得 更 复杂 。 男 一 方面 ，SPI 模式 适合 于 单 分 支 、 点 对 点 的 架 
构 ， 系 统 也 相对 简单 。 因 此 ，SPI 模式 更 适合 于 所 开发 的 系统 ， 因 为 该 系统 只 需 
与 KOALA 机 器 人 的 点 对 点 通信 ， 这 有 助 于 使 得 系统 相对 人 简单， 尤其 是 对 于 实际 
通信 的 情况 。 


2.3 KOALA 机 器 人 与 PC 和 视觉 系统 构成 的 集成 系统 


图 2. 12 给 出 了 基于 PC 的 KOALA 机 器 人 通过 PIC 16F876A 微 控制 器 板 与 视 
党 系统 中 RC 伺服 电动 机 进行 通信 的 一 个 完整 集成 系统 。PIC 微 控 制 器 与 四 个 外 
设 ( 即 视觉 系统 中 的 四 个 伺服 电动 机 ) 进行 通 信 。 这 四 个 伺服 电动 机 用 于 视觉 系 
统 的 云 台 控制 、 左 / 右 摄像 头 的 聚焦 控制 。 视 觉 系 统 所 增加 的 四 个 自由 度 及 其 有 
效 控制 提高 了 整个 集成 系统 的 灵活 性 和 自动 化 程度 。 
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PIC 
16F876A 
微 控 制 器 板 


右 摄 像 头 聚 焦 控 制 的 
伺服 电动 机 


图 2.12 由 KOALA 机 器 人 与 PC 和 视觉 系统 构成 的 集成 系统 
另外 还 开发 了 一 个 基于 PC 的 系统 来 与 KOALA 移动 机 器 人 通信 。 这 是 一 个 
通过 发 送 ASCII 码 字符 串 命 令 与 机 器 人 通信 的 GUI 系统 ， 同 时 也 可 接收 机 器 人 
返回 的 传感器 读数 。 图 2. 13 给 出 了 与 用 户 交 互 的 开发 界面 。 其 中 ， 向 用 户 提供 
了 在 串 行 传输 模式 下 驱动 命令 的 发 送 按钮 。 当 点 击 “Send ”按钮 时 ，GUI 系统 以 
串 行 模式 传输 ASCI 码 字 符 串 ， 同 时 还 在 不 同 的 显示 框 中 显示 从 机 器 人 处 接收 的 
数据 。 这 些 显示 框 有 助 于 检查 系统 是 否 以 预期 方式 执行 命令 任务 。 





Option Explicit _ 





Private Sub Commandi Click() 
Wath MSCommi 
If .PortOpen = False Then .PortOpen 


With Textz 
.SetFocus 
.SeiStarc = 0 


.5 h = Len(.Te Recieve  |ROM of minrobot KOALA. 
Ena een esCoT k= Edoardo Franzi, Pere Amaud K-T oom SA. Revision 1.18 [3 May 2002) 
End Sub RM UEM e 


oi merobot KOALA 
Private Sub Form Loadí) Edoardo Franzi Piene Amant K-T eam SA. Revision 1:19 (3 May 2002) 
With MSCommi Mal yamasa Piai 
If .PortOpen Then .P 
-ConmPort = 2 
.Settings = "38a00,N 
Send 





.SThreshold = 0 
.PortOpen = True 

End With 

End Sub 


Private Sub MSCommi OnComm(] 
Dim strInput As String 
With MSCommi 
Select Case .CommEvent 
Case comEvReceive 











图 2.13 与 用 户 交 互 的 开发 界面 
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TE KOALA 机 器 人 终端 ， 开 发 了 另 一 个 C 程序 ， 并 将 其 交叉 编译 程序 (扩展 
名 为 . s37) 下 载 到 Motorola 处 理 器 中 。 该 . s37 程序 以 一 种 总 是 需要 一 个 驱动 命令 
的 中 断 模式 与 PC 中 的 VB 程序 进行 通信 ， 即 发 送 ASCII 码 形式 的 消息 ， 并 以 一 
种 高 度 复杂 的 形式 来 响应 该 驱动 命令 。 对 于 该 串 行 通信 模式 ， 本 地 PC 作为 主机 
m KOALA 机 器 人 作为 从 机 。 主 机 PC 与 配置 为 远程 终端 单元 (RTU) 的 KOALA 机 
器 人 之 间 的 每 次 交互 都 以 如 下 方式 进行 ”: 

1) 由 主机 PC 发 送 给 机 器 人 一 个 以 回 车 符 (CR) 结 束 的 用 户 定 义 ASCH 码 字 
符 串 。 

2) 如 果 主 机 PC 命令 机 器 人 采集 和 发 送 某 个 传感器 读数 (如 红外 传感器 、 声 
纳 传感器 等 )， 则 KOALA 机 器 人 以 一 个 回 车 符 结束 的 ASCII 码 消 息 形式 返回 传 
感 器 读数 。 

图 2. 14 给 出 了 一 个 控制 KOALA 机 器 人 转向 的 简单 C 程序 示例 。 图 2. 15 给 
出 了 在 PC 中 开发 的 程序 可 通过 交叉 编译 产生 一 个 .s37 文件 ， 并 下 载 到 KOALA 
机 器 人 中 Motorola Yb FEAF AY BR EE o 


#include <sys/Kos.h> 
int 
main () 
t 
mot_reset(); 
mot config speed 1m (0, 1000, 800, 100); 


mot config speed 1m (1, 1000, 800, 100); 
mot new speed lm (0, -6); 

mot new speed lm (1, 6); 

return 0; 





K 2.14 控制 KOALA 机 器 人 转向 的 一 个 C 程序 示例 

TE KOALA 机 器 人 的 基本 套件 中 已 包含 一 组 利用 串 行 通信 协议 集 在 主机 PC 
中 打开 一 个 终端 仿真 器 并 执行 协议 命令 的 工具 和 命令 协议 。 这 些 协议 命令 包括 
一 些 非常 有 用 的 指令 ， 如 设置 速度 (“D")、 读 取 速 度 (“E")、 读 取 A-D 转换 输 
ACP), EREHE aC M") 和 读 取 接近 传感器 (“N" ) 等 。 然 而 ,除了 这 
些 KOALA 机 器 人 支持 的 指令 外 ， 还 创建 了 一 些 与 集成 外 设 进行 通信 的 协议 指 
令 。 因 此 ,创建 了 一 套 完 整 的 协议 指令 集 ， 使 得 主机 PC 以 一 种 统一 方式 来 控制 
机 器 人 。 这 些 指令 包括 与 KKOALA 机 器 人 所 支持 指令 等 效 的 指令 ， 以 及 在 本 系统 
中 创建 的 用 于 与 附加 外 设 进行 通信 的 其 他 指令 。 男 外 ,值得 注意 的 是 ， 当 在 PC 
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PC KOALA 机 器 人 









为 交叉 编译 在 
< 程序 中 添加 头 
文件 <sys/Kos.h> 








通过 串口 RS232C 
通信 进行 下 载 






Motorola 
68331 


处 理 器 





编译 程序 以 生成 
一 个 扩展 名 为 S37 
的 可 执行 文件 






图 2. 15 ”下载 一 个 交叉 编译 . c 程序 的 顺序 操作 的 框图 

的 终端 仿真 器 中 执行 KOALA 机 器 人 支持 的 指令 时 ， 首 先 应 进入 机 器 人 处 理 器 的 
监控 程序 模块 并 执行 其 设计 功能 。 然 而 ， 当 一 个 . 837 程序 下 载 到 Motorola 处 理 
器 中 与 PC 端 和 PIC 微 控制 器 端 进行 通信 时 ， 将 无 法 从 终端 仿真 器 激活 监控 程 
序 。 因 此 ， 将 所 有 机 器 人 协议 指令 处 于 同一 框架 ( 即 从 VB 程序 激活 并 由 . s37 FE 
序 执行 ) 下 是 非常 重要 的 。 因 此 ， 为 KOALA 机 器 人 编写 的 C 程序 必须 包含 所 有 
由 PC 端 发 送 的 ASCH 码 请 求 消息 。 一 组 指令 的 示例 列表 以 及 主机 发 送 的 ASCII 
码 消息 形式 见 表 2. 1。 

表 2.1 主机 PC 和 KOALA 机 器 人 之 间 进 行 串口 通信 的 命令 协议 

命令 性 质 命令 协议 (ASCIL 码 字符 串 消息 ) 命令 类 别 
设置 KOALA 机 器 人 的 电动 机 速度 





«D» «s» «ddd » «s» «ddd » «CR» KOALA 机 器 人 的 等 效 指令 
设置 KOALA 机 器 人 的 到 达 位 置 «C» «s» <ddddd > «s» «ddddd > <CR > 执行 
请 求 发 送 红 外 接近 传感器 数据 «N» «CR» 


请 求 发 送 红 外 环境 光 传 感 器 数据 << O > «CR» 执行 
请 求 发 送 声 纳 传感器 数据 <l> «CR» 执行 
请 求 发 送 电 动机 速度 信息 «E» «CR» 执行 
设置 电动 机 位 置 «G» «CR» 执行 
请 求 发 送 电动 机 位 置信 息 «H» «CR» 执行 
设置 RC 伺服 电动 机 的 位 置 命令 <Z><i><s><dd><CR> 附加 的 外 设 接口 命令 


在 表 2. 1 中 ， 例 如 要 设置 KOALA 机 器 人 的 电动 机 速度 ， 可 以 首先 是 标识 符 
“D”， 然 后 是 由 一 个 三 位 数字 设置 的 每 台电 动机 的 速度 (KOALA 机 器 人 是 一 个 差 
动 驱动 系统 )。 其 中 ， 先 是 “Motor 0 的 速度 指令 ， 然后 是 “Motor 1 的 速度 指令 。 
由 于 KOALA 机 器 人 中 每 台电 动机 速度 的 极 性 可 分 别 设置 为 前 向 运动 和 后 向 运 
动 ， 因 此 在 各 台电 动机 的 速度 信息 之 前 增加 一 个 符号 位 ， 在 指令 协议 中 表示 为 
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<s>。 设 ”+ 表示 前 向 运动 ,”- “表示 后 向 运动 。 当 Motorola 处 理 器 接收 到 这 
个 ASCI 码 字符 串 消 息 后 ，. s37 程序 就 开始 进行 字符 串 解 码 并 根据 指令 协议 通 
过 驱动 KOALA 机 器 人 的 每 个 电动 机 来 执行 该 指令 。 执 行 每 个 KOALA 机 器 人 等 
效 指令 (作为 一 个 ASCI 码 字 符 串 消息 由 PC 发 送 ) 时 也 是 类 似 的 动作 。 

然而 ， 当 Motorola 处 理 器 接收 到 附加 外 设 接口 命令 相应 的 ASCI 码 字 符 串 消 
息 时 ， 将 正确 地 解码 协议 指令 ， 并 向 PIC 处 理 器 发 送 合适 的 驱动 指令 。 在 这 种 
情况 下 ， 如 图 2.7 Aras, Motorola 处 理 器 工作 在 主 模式 下 ， 而 PIC 处 理 器 工作 在 
从 模式 下 。 对 于 由 PC 发 出 的 该 行为 的 字符 串 消息 ， 首 先是 标识 符 “Z”， 接 下 来 
是 位 置 命令 所 对 应 的 伺服 电动 机 ID «i» (i 从 1 变化 到 4)， 然 后 是 位 置 命令 的 符 
号 位 <s>(“+ "或 “-'), 最 后 是 单位 为 度 的 表示 实际 位 置 命令 的 两 位 数字 
( -90° ~90°)。 每 个 ASCI 字符 串 消息 都 以 回 车 符 < CR > 结束, 一旦 Motorola 
处 理 器 接收 到 该 字符 串 ， 就 对 该 字符 串 进 行 解码 来 产生 电动 机 ID 的 一 个 字 节 信 
息 ， 以 及 相应 角 位 置 命 令 的 一 个 字 信 息 (0° 对 应 1500ps, - 90" 对 应 1000hs， 
+90° 对 应 2000pjs， 每 个 中 间 角 位 置 命令 对 应 成 比例 的 时 间 内 插值 )， 然 后 该 字 
信息 进行 分 解 以 分 别 产生 相应 的 高 字 节 信息 和 低 字 节 信 息 


2.4 实际 性 能 评估 


通过 从 主机 PC 端 ( 即 从 VB 程序 端 ) 串 行 通信 向 KOALA 机 器 人 发 送 ASCH 
码 消息 指令 来 对 所 有 协议 指令 进行 性 能 评估 。 在 进行 评估 实验 之 前 ， 分 别 单独 
测试 对 每 个 RC 伺服 电动 机 的 校准 ， 并 引入 软件 校正 来 补偿 小 的 偏 移 误差 。 进 行 
了 一 系列 实验 来 测试 机 器 人 集成 系统 实际 执行 ASCH 码 字 符 串 消息 所 发 布 的 任务 
的 情况 ， 其 中 一 些 实验 情况 描述 如 下 。 

在 一 个 实验 中 ， 为 测试 驱动 运动 ， 要 求 机 器 人 以 某 一 较 小 的 均匀 速度 前 向 
运动 。 图 2. 16 给 出 了 PC 上 GUI 系统 的 程序 界面 ， 这 时 发 送 的 ASCII 码 字 符 串 
消息 为 "D +002 +002”。 可 看 出 机 器 人 运动 与 所 执行 的 命令 一 致 。 同 理 ， 在 较 高 
运动 速度 指令 、 反 向 运动 指令 以 及 电动 机 差 动 驱动 指令 下 来 测试 这 些 指 令 ， 同 
样 可 看 出 该 机 器 人 集成 系统 的 行为 与 各 个 指令 一 致 。 

接 下 来 ， 进 行 另 一 组 实验 ， 主 要 测试 主机 PC 5; KOALA 机 器 人 之 间 的 双向 
串 行 通信 。 在 这 些 实验 中 ， 要 求 机 器 人 读 取 传感器 读数 ， 并 将 其 返回 给 主机 PC. 
PC 端 发 出 ASCII 48 4 * N "来 读 取 安 装 在 机 器 人 周围 所 有 16 个 红外 接近 传感器 的 
读数 。 这 些 红外 传感器 的 测 距 范围 是 5 ~20cm。 由 红外 传感器 测量 的 每 个 输出 值 
都 是 一 个 经 10 位 A-D 转换 后 的 模拟 量 。 在 红外 传感器 LO 和 R0( 机 器 人 轴线 方 
向 上 放置 的 两 个 红外 传感器 ) 前 方 分 别 距离 5cm 和 20cm 处 放置 一 个 Secm 宽 的 障 
碍 物 。 实 验 情 况 与 实际 情况 相符 ,距离 较 近 时 ， 返回 的 传感器 读数 较 大 。 当 距 
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Private Sub Commandi Cl 
With MSCommi 
If .PortOpen = False 
.Output = Text2,.Text | 
End With i] 
With Text2 
-SetFocus 
.SelStart = 0 
.SeiLlength = Len(.Tex 
End With 
| End Sub - 














Private Sub Form Load() 

With MSCommi ana 
1f .PortOpen Then .Po pines 
.CommPort = 2 
.Settings = "38400,N,. 


-DTREnable ~ True 
.RTSEnable = True ) 
.RThreshoid = i i 


.SThreshold = 0 
.PortOpen = True 

End With 

End Sub — 











Private Sub MSCommi OnComm() 
Dim strInput As String 
With MSCommi 

Select Case .CommEventc 
Case comEvReceive 


2.16 对 机 器 人 集成 系统 执行 前 向 驱动 命令 时 的 GUI 程序 界面 
BAA Sem tht, LO 和 RO 返回 的 读数 为 1023 ， 这 是 可 能 的 最 大 值 。 其 他 红外 传 


感 器 的 读数 也 以 字符 串 形 式 获 得 ， 并 在 主机 PC 端 显示 ， 如 图 2. 17a 所 示 。 由 图 
est insole Cees ase ean ION 人 a8 











































Option Explicit — 





Projecti (Projecti.vbp) 
Private Sub Commandi_Click() Forms 
Wath MSCommi 
If .PortOpen = Faise Then 
„Output = Text2.Te 
End With 
With Text? 
.SetFocus 
.SelStart = 0 
.SelLength = Leni.T 
End With ' 
End Sub —— 


P3 Formi (Fort frm) 


Recieve 


Private Sub Form Load(| 
With MSComml 
If .PortOpen Then . 
.CommPort = 2 
“Sercings = "38400,. send F 





.DTREnable = True 
,RTSEnable = True 
-RThreshold = 1 


.SThreshold = 0 1 j ? 
.PorrOpen = True Bend 
End With 


Zad wun 





Private Sub MSCosmi OnComm(| 
Dim strinput às String 
With mSConmi 

Select Case .CommEvent 
Case comEvReceive 











a) 


图 2.17 对 于 一 个 5cm 宽 的 障碍 物 ， 机 器 人 集成 系统 分 别 在 距离 Scm 和 20cm 
处 执行 读 取 红 外 传感器 数据 命令 时 的 GUI 程序 界面 
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tas 
Project! (Project 1.vbp) 
=) Gi Forms 


Private Sub Commandi Click () 
EY Formi (Fori frm) 


With HSCommi 
If .PortOpen = False Then .PortOpen = True 
.Output * Text2.Text & vbCr 

End With 
With Text2 D 
.SetFocus ATA 
.SelStart = 0 | 
.SelLength = Len(.Tex’ 
End With ' 1 
Ind Sub — 





Private Sub Form Load() 
With MSConml ; 
1f .PortOpen Then .Po: 
.CommPort = 2 
Settings = "38400,N,+ 
.DTREnable = True 





.RThreshold = 1 
.SThreshold = 0 
.PortOpen = True 
End With 
|End Sub 00 











Private Sub NSComwl OnGonigemmm tn 
Dim strInput As String 
With NSCommi 
Select Case .CommEvent 
Case comEvReceive 





b) 
Kd 2.17 对 于 一 个 5cm 宽 的 障碍 物 ， 机 器 人 集成 系统 分 别 在 距离 Sem 和 20cm 
处 执行 读 取 红 外 传感器 数据 命令 时 的 GUT 程序 界面 ( 续 ) 

可 知 ， 对 于 其 他 红外 传感器 ,在 给 定 的 障碍 物 位 置 下， 由 于 每 个 传感器 与 障 但 
物 之 间 的 距离 大 于 LO 和 RO 与 障碍 物 之 间 的 距离 ， 因 此 传感器 读数 较 小 。 知 障 
得 物 与 机 器 人 朝向 之 间 的 距离 大 于 20cm， 则 LO 和 RO 所 读 取 的 读数 分 别 为 253 
和 348 ， 如 图 2. 17b 所 示 。 相 应 地 ， 从 其 他 传感器 获得 的 读数 也 显著 减 小 。 同 
HE, pp PC 端 发 出 ASCII 指令 “1’ 可 获得 两 个 声 纳 传 感 絮 的 读数 。 这 些 声 纳 传 感 
器 可 用 于 检测 距离 15 ~300cm 的 障碍 物 。 相 应 的 模拟 输出 量 为 0 ~4.096V。 对 应 
的 数字 量 为 0 ~1023 ， 其 中 0 对 应 0V( 最 小 距离 ) ， 而 1023 对 应 4.096V( 最 大 距 
离 ) 。 机 器 人 分 别 位 于 距离 墙 面 100cm 和 200cm 处 所 获得 的 读数 分 别 如 图 2. 18 
所 示 。 当 与 墙 距离 100cm 时 ， 返 回 的 声 纳 读数 分 别 为 403 和 417， 而 当 与 墙 距离 
200cm 时 ， 返 回 的 声 纳 读数 要 大 得 多 ， 分 别 是 885 和 895 。 

接 下 来 ， 进 行 的 另 一 组 重要 实验 是 附加 外 设 的 接口 指令 ， 这 时 ， 主 机 PC 端 
(工作 于 主 模式 ) 发 出 的 串口 指令 以 ASCH 标识 符 “Z 开始 。 该 指令 由 从 模式 下 
KOALA 机 器 人 中 的 Motorola 处 理 器 接收 ， 随 后 Motorola 处 理 器 (这 时 工作 于 主 模 
式 ) 对 每 个 伺服 电动 机 发 出 指令 ,通过 PIC 微 控 制 顺 板 来 初始 化 溃 行 通信 ， 其 中 
PIC 16F876A 微 控 制 器 工作 于 从 模式 。 图 2. 19a 给 出 了 每 个 伺服 电动 机 位 于 中 间 
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= I Projecti (Projectt.vbp) | 
= S5 Forms 


D Forni (Formit frm) 





Private Sub Commandi Click() 
With M3Comml 
If .PortOpen = False Then .PortOpen = True 
„Output = Text2.Text & vbCr 
End With 
With Text2 
-SetFocus 
-SelStart = O 
.SelLength = Len(.Text) 


Private Sub Form Load() 
With MSCommi 
If .PortOpen Then .PortOpen * 
.CommPort = 2 
.Settings = "38400,N,8,2" 
,DTPEnable = True 
-RTSEnable = True 
.RThreshold = 1 
-SThreshold = 0 
-PortOpen = True 
End vith 
L..— End mus od 
Private Sub MSCommi OnComm() 
Dim strInput As String 
With MSCommi 
Select Case .CommEvent 
Case comEvReceive 




























| Option Explicit 


Private Sub Commendi Ciick() 
With MSCommi 





With Text2, I 
.Setfocu p 885.285 | 
-Seister 
.SelLeng 

End With 

End Sub 





Private Sub * 
Wath MSCom 4 








:SThresh 
-PortOpe, 
End With 





Private Sub MSCommi1_OnConm() 
Dim strinput As String 
Wath MSCommi 

Select Case .CommEvent 
Case comEvReceive 





b) 


图 2.18 机 器 人 集成 系统 分 别 在 距离 墙 100cm 和 200em 处 执行 读 取 声 纳 
传感器 数据 命令 时 的 GUI 程序 界面 
位 置 时 的 初始 状态 。 然 后 ,分 别 依次 发 出 四 个 指令 “Zl +45", 622445", ‘73 
+45 ”和 “24 +45" ， 使 得 用 于 左 摄像 头 聚焦 控制 、 右 摄像 头 聚 焦 控 制 、 水 平 控制 
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和 俯仰 控制 的 每 个 伺服 电动 机 相应 地 依次 旋转 +45°*。 图 2.19b ~e 给 出 了 主机 
PC 端 分 别 发 出 每 个 指令 后 系统 的 程序 界面 。 这 些 实验 表明 在 实际 应 用 中 ， 与 
KOALA 机 器 人 集成 的 附加 外 设 可 成 功 地 与 机 器 人 接口 ， 并 按 预 期 从 主机 PC m 
获得 合适 的 指令 。 表 2.2 给 出 了 从 PC 端 ( 在 此 用 户 发 出 指令 ) 发 出 指令 的 时 间 延 
38, UR RED RC 伺服 电动 机 按 指令 完全 执行 其 功能 。 对 于 每 个 RC 伺服 电动 机 
分 别 进行 角 位 置 指令 为 45°* 和 90° 的 两 次 实验 。 由 表 2.2 可 知 ， 左 摄像 头 聚 焦 轴 、 
右 摄像 头 聚焦 轴 和 偏转 轴 的 感应 和 反应 延迟 非常 接近 。 而 相对 于 其 余 三 个 自由 
度 ， 水 平 轴 在 实际 中 的 时 延 较 大 。 





图 2.19 初始 化 条 件 下 [ 即 每 个 伺服 电动 机 均 位 于 0? 角度 位 置 ( 称 为 中 心 位 置 ) ] 
的 系统 配置 图 片 ( 见 图 2.19a) 和 由 PC 依次 发 送 的 对 于 四 个 分 别 进行 左 
摄像 头 聚焦 控制 、 右 摄像 头 聚 焦 控制 、 水 平 控制 和 俯仰 控制 的 RC 伺服 

电动 机 执行 45° 角 位 置 指令 时 的 系统 位 置 图 片 ( 见 图 2. 19b ~ e) 
表 2.2 RC 伺服 电动 机 感应 和 反应 延迟 时 间 


























自由 度 发 送 的 角 位 置 指令 /(°) 感应 和 反应 延迟 时 间 /s 
90 0. 433 
左 摄像 头 聚 焦 轴 
45 0. 200 
90 0. 433 
右 摄 像 头 聚焦 轴 
45 0. 200 
90 0. 633 
IKE fil 
45 0. 333 
90 0. 400 
偏转 轴 - 
45 0. 300 
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2.5 本 章 小 结 


本 章 详细 介绍 和 阐述 了 如 何 开 发 一 个 基于 PIC 微 控 制 咒 的 系统 用 于 与 现 有 
移动 机 器 人 (在 此 是 KKOALA 机 器 人 ) 的 实际 外 设 接口 。 在 所 开发 的 系统 中 ， 以 中 
断 驱动 模式 进行 串 行 通信 ， 其 中 KOALA 机 器 人 的 处 理 咒 工作 于 主 模 式 mi PIC 
处 理 器 工作 于 从 模式 。 在 实验 室 环境 下 开发 了 一 个 由 PC, KOALA flat A, PIC 
微 控 制 器 板 和 基于 双 摄 像 头 的 视觉 系统 组 成 的 完整 集成 系统 。 其 中 ，PIC 微 控制 
器 系统 是 用 于 控制 视觉 系统 中 四 个 自由 度 的 四 个 外 设 ( 即 四 个 PC 伺服 电动 机 ) 。 
该 完整 集成 系统 在 PC 的 GU 系统 控制 下 工作 ， 其 中 PC 端的 系统 工作 在 主 模式 ， 
并 以 中 断 驱 动 模式 与 工作 于 从 模式 的 KOALA 机 器 人 中 的 Motorola fb PE qs uEfT th 
行 通信 。 

所 开发 的 系统 表明 如 何在 机 器 人 基本 平台 的 外 部 集成 附加 外 设 来 提高 集成 
系统 的 能 力 和 复杂 性 ， 且 开发 成 本 低 。 这 样 的 思路 也 可 扩展 到 增加 更 多 的 其 
他 外 设 ， 同 时 也 有 助 于 在 其 他 实际 移动 机 器 人 上 实现 。 另 外 ， 也 和 希望 本 童 中 
对 该 系统 的 详细 介绍 能 有 助 于 机 器 人 领域 的 研究 人 员 来 开发 类 似 的 集成 系统 。 
由 于 通常 情况 下 ， 一 个 机 器 人 集成 平台 中 的 集成 传感器 和 其 他 部 件 的 技术 说 
明 书 都 只 由 生产 广 商 掌握 ， 而 无 法 公开 获得 ， 因 此 这 将 对 开展 机 融 人 研究 非 
常 有 用 。 同 时 ， 还 应 注意 到 ， 尽 管 在 此 所 给 出 的 所 有 实验 都 是 基于 双 摄 像 头 
系统 进行 的 ， 但 该 系统 同样 也 适用 于 单 摄像 头 系统 ， 这 时 将 摄像 头 放置 在 主 
动 式 云 台 系 统 的 中 央 位 置 ， 这 种 情况 下 ， 只 能 控制 两 个 自由 度 ( 仅 水 平 控制 和 
俯仰 控制 ) 。 

致谢 本章 介绍 的 研究 工作 得 到 了 印度 大 学 教育 资助 委员 会 重大 研究 项 目 
计划 的 资助 (资助 号 : 32-118/2006 (SR) ] 。 
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摘要 : 本 章 介 绍 了 如 何 开发 一 个 两 层 架 构 且 与 红外 传感器 相 结 合 的 基于 视 
觉 的 机 器 人 导航 机 制 。 该 算法 采用 一 种 基于 子 目标 的 机 制 来 通过 跟踪 最 短路 径 
以 达到 最 终 目 标 ， 并 同时 实现 预期 的 避 障 功能 。 以 创建 下 一 个 子 目 标的 形式 和 迭 
代 运 行 该 算法 ， 并 在 单 次 迭代 中 导航 到 该 点 ， 从 而 尽 可 能 地 在 最 少 和 迭代 次 数 下 
达到 最 终 目 标 。 


3.1 简介 


机 器 人 领域 的 一 些 最 新 技术 发 展 对 许多 工业 和 社会 领域 做 出 了 重大 贡献 。 
目前 ， 在 工 广 自动 化 、 监 控 系 统 、 质 量 控制 系统 、 自 主导 引 车 (ACV) 、 防 灾 和 
医疗 辅助 等 领域 都 有 着 机 器 人 系统 的 大 量 应 用 。 现 在， 越 来 越 多 的 机 器 人 应 用 
旨 在 提高 人 类 日 常生 活 ， 并 且 机 器 人 也 正 前 所 未 有 地 执行 各 种 不 同 任务 ”。 对 
于 许多 这 样 的 应 用 ， 机 器 人 的 自主 移动 能 力 是 其 中 一 个 关键 因素 。 许 多 现代 机 
器 人 的 应 用 中 ， 都 采用 计算 机 视觉 作为 主要 的 感知 手段 。 如 前 所 述 ， 视 觉 系统 
可 看 作 一 种 被 动 传感器 ， 它 比 红 外 、 激 光 和 声 纳 等 主动 传感器 具有 更 多 的 优点 。 
被 动 传 感 顺 (如 摄像 头 ) 在 采集 数据 过 程 中 发 射 光 或 波 不 会 改变 环境 ， 并 且 在 所 
获得 的 图 像 中 包含 比 主动 传感器 更 多 的 信息 (如 物质 、 空 间 和 时 间 信 息 )”。 视 
觉 是 能 够 让 人 类 提取 客观 物理 世界 相关 信息 的 一 种 感知 ， 同 时 也 是 人 类 最 依赖 
的 感知 。 能 够 提取 这 些 信息 的 计算 机 视觉 技术 不 断 发 展 ， 现 今 移动 机 器 人 已 实 
现 了 越 来 越 多 的 基于 视觉 的 实时 导航 系统 。 

基于 视觉 的 机 器 人 导航 是 一 种 能 够 引导 移动 机 器 人 到 达 预 期 位 置 ， 或 沿 环 
境 中 一 条 期 望 路 径 运 动 ， 并 主要 利用 视觉 传感器 避 开 静态 障碍 物 (或许 是 动态 障 
FEL) 的 技术 ””。 本 章 将 介绍 一 种 移动 机 器 人 导航 机 制 的 具体 实现 ， 其 中 视觉 
感知 作为 路 径 规 划 的 主要 传感器 ， 而 红外 传感器 作为 在 一 个 静态 或 动态 室内 环 
境 中 具有 避 障 功能 的 移动 机 器 人 实际 导航 的 辅助 传感器 。 如 前 所 述 ， 构建 环境 





C) 本 章 是 基于 N. Nirmal Singh, Avishek Chatterjee, Amitava Chatterjee 和 Anjan Rakshit 2011 年 5 月 发 
K F Measurement 期 刊 第 44 卷 ， 第 4 期 ，620 ~641 页 的 论文 “A two-layered subgoal based mobile ro- 


bot navigation algorithm with vision system and IR sensors”。 经 Elsevier 出 版 商 许 可 后 重印 。 
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地 图 的 常用 方法 基于 机 格 法 “””、 拓 扑 地 图 * 和 混合 地 图 '” 等 。 无 地 图 导航 系 
统 是 指 在 整个 导航 空间 中 无 需 显 式 表 示 ， 而 是 通过 识别 环境 中 的 物体 ， 并 根据 
视觉 观测 产生 运动 命令 来 跟踪 或 避 开 这 些 物 体 的 导航 系统 "”。 另 外 还 有 一 些 
研究 工作 提出 利用 立体 视觉 、 三 目 视 觉 ”、 全 方位 或 全 景 视觉 ，" 和 单 目 
视觉 ”来 获取 环境 信息 。 移 动机 器 人 导航 中 的 每 种 方法 都 有 各 自 的 优点 和 缺 
点 。 在 地 图 知识 已 知 的 情况 下 ， 导 航 的 一 个 重要 问题 是 用 于 移动 机 器 人 智能 控 
制 或 引导 的 路 径 规划 问题 。 路 径 规 划 的 常用 方法 是 基于 地 图 法 、 栅 格 分 解法 和 
势 场 法 等 方法 ”， 不 同 之 处 在 于 如 何 构 建 连通 图 及 其 表示 。 显 然 ， 如 果 没 有 环 
境 的 任何 先 验 知识 ， 就 几乎 不 可 能 在 所 有 可 能 路 径 中 确定 最 短 的 导航 路 径 。 确 
定 最 短路 径 可 采用 标准 的 图 搜索 技术 ， 如 Dijkstra 算法 ”和 A* 算 法 ”， 

如 上 所 述 ， 本 章 将 介绍 一 种 目标 驱动 方法 来 进行 移动 机 器 人 导航 ， 即 采用 
视觉 感知 和 红外 导航 ，“” 。 在 一 个 存在 障碍 物 的 静态 或 动态 环境 中 ， 这 种 基于 
两 层 的 方法 可 确定 起 始点 和 已 知 目标 点 之 间 的 最 短 导 航路 径 。 在 第 一 层 中 ， 视 
觉 作为 主要 感知 系统 为 随后 的 路 径 规划 采集 环境 图 像 。 首 先 ， 对 所 采集 的 图 像 
执行 一 系列 的 图 像 处 理 操作 ， 然 后 采用 梯度 下 降 算法 来 计算 机 器 人 当前 位 置 与 
目标 点 之 间 的 最 短路 径 ， 并 实现 避 障 O 利用 该 最 短路 径 来 产生 子 目 标 ， 然 后 
在 基于 红外 传感器 的 导 引 基础 上 ， 利 用 这 些 子 目标 信息 来 进行 局 部 导航 。 即 使 
在 环境 动态 变化 的 情况 下 ， 第 二 层 中 基于 红外 传感器 的 机 器 人 导航 也 能 够 导 引 
机 器 人 到 达 子 目标 。 一旦 机 器 人 达到 子 目 标 ， 就 再 次 激活 该 两 层 算法 来 产生 一 
个 新 的 子 目标 ， 并 将 机 器 人 导航 到 新 的 子 目标 。 重 复 执行 该 过 程 ， 直 到 达到 最 
终 目标 。 该 方法 想 要 同时 实现 两 个 目标 。 首先 ， 基 于 视觉 感知 ， 要 力图 实现 一 
个 最 短路 径 规划 算法 ， 以 尽 可 能 快 地 在 避 障 的 同时 达到 目标 点 、 然后， 如 果 在 
导航 过 程 中 环境 发 生变 化 上 且 障 碍 物 信息 更 新 ， 则 基于 红外 传感器 的 导 引 将 使 得 
机 器 人 具有 应 对 环境 变化 的 能 力 ， 从 而 保证 机 器 人 仍然 能 够 安全 导航 。 定 期 利 
用 视觉 来 更 新 环境 信息 ,那么 随后 的 路 径 规划 和 红外 导航 就 能 够 帮助 机 器 人 在 
导航 过 程 中 适应 环境 的 动态 变化 ， 并 在 尽 可 能 短 的 时 间 内 到 达 目 标点 。 本 实验 
室 开发 的 算法 可 对 KOALA 机 器 人 创建 几 种 实际 环境 ” 。 实验 结果 表明 了 所 提 
算法 的 有 效 性 。 该 算法 将 在 本 章 随 后 的 内 容 中 详细 介绍 。 


3.2 硬件 设置 


第 2 章 已 详细 介绍 了 KOALA 机 器 人 。 在 此 ， 再 次 简要 概述 一 下 KOALA 机 
器 人 的 突出 特点 ， 并 简单 介绍 在 实际 实现 中 所 用 的 硬件 设置 。KOALA 机 器 人 是 
一 个 由 瑞士 K-team 公司 生产 的 六 轮 差 动 驱动 小 车 (32cm x32cm) ” o KERK 
所 用 的 KOALA 机 器 人 配置 有 16 个 接近 /环境 红外 传感器 、 四 个 声 纳 传感器 和 车 
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轮 编 码 器 ， 并 在 加 尔 各 答 贾 达 普 (Jadavpur) 大 学 电子 工程 系 的 电子 测量 与 仪器 实 
验 室 中 对 该 机 器 人 集成 了 两 个 完整 视觉 系统 。 所 开发 的 视觉 系统 既 可 用 作 采 用 
双 摄 像 头 的 立体 视觉 系统 (如 第 2 章 所 述 ) ， 也 可 用 作 采 用 单 摄像 头 的 视觉 系统 
现在 ， 介 绍 一 种 采用 一 个 无 线 摄像 头 作为 单 目 视觉 的 算法 。 在 KOALA 机 器 人 
中 ， 利 用 一 个 车 载 微 控制 器 ( Motorola 68331， 工 作 频 率 为 22MHz) 来 进行 硬件 控 
($7 。 图 3. 1a 给 出 了 实验 室 所 采用 的 集成 了 四 个 声 纳 传感器 和 由 单 目 视觉 构成 
的 视觉 系统 的 移动 机 器 人 图 片 。 其 中 ， 声 纳 传 感 器 可 检测 15 ~ 300cm 范围 内 的 
障碍 ， 而 红外 传感器 的 测量 范围 为 5 ~20cm。 本 系统 采用 的 单 目 视 觉 系统 是 由 一 


USB 
图 像 采 集 卡 





b) 


图 3.1 配置 有 声 纳 和 红外 传感器 并 集成 了 基于 单 摄像 头 的 视觉 系统 的 KOALA 机 器 人 
CILE] 3. 1a) 以 及 视觉 系统 的 框图 ( 见 图 3. 1b) 
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个 具有 A/V 接收 器 的 JMK 无 线 摄像 头 ( WS-309 AS ) 和 一 个 可 采集 连续 视频 流 的 
Frontech USB 图 像 采 集 卡 (TV Box) 组 成 的 。 图 3. 1b 给 出 了 该 视觉 系统 的 原理 示 
意图 。 整 个 系统 开发 的 目标 是 提供 一 个 在 工业 领域 具有 吸引 力 的 低 成 本 解决 方 
案 ， 所 开发 的 单 目 视觉 系统 应 具有 可 进行 云 台 控制 的 两 个 自由 度 。 为 对 该 视觉 
系统 增加 两 个 自由 度 ， 本 实验 室 开发 了 一 个 PIC(16F876A ) HL FE tl dis AR BK XT Ak 
于 单 目 摄像 头 的 机 器 人 系统 进行 云 台 控制 ”。 在 此 ， 车 载 Motorola 微 控制 器 作 
为 主机 ， 而 PIC 微 控制 器 作为 从 机 。 以 中 断 驱 动 模式 开发 的 软件 可 实现 通过 
RS232C 与 移动 机 器 人 进行 通信 。 图 3.2 给 出 了 PC 端 开发 的 可 与 用 户 交 互 的 用 
户 接口 程序 界面 。 主 要 的 串口 通信 是 由 PC 与 机 器 人 Motorola 处 理 器 之 间 的 
ASCII 和 ih o 
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图 3.2 可 与 用 户 交 互 的 用 户 接口 程序 界面 














PC 与 机 器 人 之 间 的 RS232C 串口 连接 通常 设置 为 一 个 起 始 位 、 两 个 停止 位 
且 无 奇偶 校 验 位 模式 的 Shit 数据 。 例 如 ， 由 PC 端 发 送 的 用 于 云 台 控制 的 RC fii] 
E M d indie qi Ea bec 
ID <i>(i=1，2)， 接 下 来 是 位 置 命 令 的 符号 位 <s>( t3 -7), 最 后 是 两 
个 实际 的 位 置 命令 位 ， 单 位 是 度 ( 可 从 -90° 变 化 到 +90°)。 通 常 ， 由 回 车 符 
< CR > 来 结束 每 个 ASCI 人 码 字符 串 消息 。 
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3.3 目标 导向 的 两 层 导航 机 制 


图 3.3 给 出 了 所 提出 的 完整 导航 算法 的 流程 。 如 图 3. a 所 示 的 一 个 无 线 摄 
像 头 用 于 采集 KOALA 机 器 人 前 方 环境 的 连续 视频 流 。 对 该 视频 流 中 任意 时 刻 的 
一 帧 采集 图 像 进一步 处 理 。 首 先 对 采集 图 像 帧 进行 一 系列 图 像 处 理 操作 ， 如 图 
像 滤 波 、 边 缘 检测 和 图 像 分 割 等 ， 使 得 图 像 适 合 于 进一步 处 理 。 然 后 ， 根 据 所 
提供 的 先 验 目 标 信 息 ， 对 处 理 后 的 图 像 采 用 最 短路 径 生 成 算法 。 接 下 来 ， 利 用 
所 产生 的 最 短路 径 来 确定 下 一 个 子 目标 。 整 个 过 程 构成 了 算法 的 第 一 层 ， 并 在 
PC 中 利用 VB 平台 高 层 实现 。 然 后 ,将 该 子 目 标 信息 传 给 KOALA 机 器 人 利用 避 
障 功能 朝子 目标 实际 导航 的 算法 第 二 层 ， 从 而 实现 机 器 人 在 一 个 动态 变化 环境 


开始 


获取 目标 信息 





从 摄像 头 采 集 的 连续 视频 流 中 获取 图 像 帧 








| 执行 图 像 处 理 操作 ; ”第 一 层 





对 已 处 理 图 像 执行 最 短路 径 算法 


从 生成 的 最 短路 径 中 提取 下 一 个 子 目标 


2 


DY ooo on oan hn = oe eee ee ee ee ny 








图 3.3 所 提 导 航 算法 的 流程 
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中 的 运动 。 第 二 层 中 的 导航 是 利用 几 个 安装 在 KOALA 机 器 人 前 方 和 两 侧 的 红外 
传感器 来 实现 的 。 一 旦 机 器 人 到 达 子 目标 ， 就 又 返回 到 第 一 层 控 制 ， 从 而 产生 
下 一 个 子 目 标 ， 并 在 第 二 层 中 进行 导航 。 迭 代 执 行 这 种 局 部 路 径 规 划 和 实际 导 
航 的 过 程 ， 直 到 达到 最 终 目 标 。 第 二 层 中 实际 导航 的 算法 是 由 一 个 下 载 到 KOA- 
LA 机 器 人 中 Motorola 处 理 器 的 交叉 编译 C 程序 (一 个 .s37 文件 ) 实 现 的 ， 该 
.537 文件 与 PC 端的 VB 程序 以 中 断 驱 动 方式 通信 。 由 C 程序 生成 的 . s37 程序 还 
可 对 PC 端的 VB 程序 提供 一 组 控制 KOALA 机 器 人 的 协议 命令 ， 这 些 命令 原本 
只 适用 于 在 KOALA 机 器 人 套件 提供 的 终端 仿真 器 中 执行 。 在 此 ， 开 发 了 一 个 由 
C 程序 执行 的 支持 所 有 KOALA 机 器 人 协议 指令 以 及 附加 导航 算法 的 系统 ， 使 得 
整个 系统 都 能 够 完全 由 PC 端的 VB 程序 控制 。 


3.4 第 一 层 中 基于 图 像 处理 的 环境 探索 


图 像 处 理 是 一 种 以 图 像 为 输入 信号 ， 输 出 信号 为 输入 图 像 变换 形式 的 信和 号 
处 理 形 式 。 本 书 所 提出 的 系统 采用 一 种 基于 图 像 分 割 的 地 图 构建 方法 ， 用 于 室 
内 环境 下 移动 机 器 人 的 视觉 导航 ， 并 假设 表面 一 致 。 具 体 实现 步 又 如 下 ”: 

l. 从 无 线 摄像 头 采 集 图 像 

安装 在 机 器 人 云 台 系统 中 心 的 摄像 头 不 断 采 集 机 器 人 前 方 环境 的 视频 流 。 
可 从 该 视频 流 中 采集 一 帧 图 像 来 进一步 处 理 。 图 3. 4a 给 出 了 一 帧 采集 图 像 。 

2. 对 采集 图 像 进 行 低 通 滤 波 

接 下 来 ， 对 采集 图 像 进行 低 通 滤 波 以 减少 噪声 。 该 处 理 可 对 邻近 像素 点 产 
生平 滑 或 模糊 效果 。 本 书 所 开发 的 系统 采用 常用 的 算术 均值 滤波 来 进行 低 通 滤 
波 ， 该 算术 均值 滤波 对 每 个 像素 点 中 心 采 用 一 个 5 x 5 矩阵 ， 并 将 该 滤波 和 矩阵 作 
用 下 的 像素 点 平均 值 作为 该 像素 点 的 亮度 值 。 

3. 对 滤波 图 像 进 行 Canny 边缘 检测 

边缘 表示 不 同 灰 度 值 的 两 个 区 域 之 间 的 边界 。 基 于 边缘 的 分 割 技术 是 指 利 
用 不 同类 型 的 梯度 算 子 从 场景 中 分 离 出 期 望 目标 。 在 本 系统 中 采用 Canny 边缘 
检测 算法 来 检测 图 像 中 的 边缘 。 图 3.4b 给 出 了 经 低 通 滤波 处 理 后 采集 图 像 的 边 
缘 图 像 。 

4. 加 粗 并 连接 边缘 图 像 中 的 边缘 

边缘 图 像 中 包含 许多 小 的 杂 散 边缘 。 为 使 得 边缘 图 像 更 有 意义 ， 需 要 将 相 
邻 边缘 连接 并 加 粗 来 使 之 更 加 明显 。 通 过 一 种 称 为 膨胀 的 形态 学 操作 来 进行 边 
缘 加 粗 处 理 ， 即 利用 一 个 可 增长 图 像 中 选择 区 域 的 前 景 像 素 的 结构 元 素 。 通 常 
是 对 一 幅 二 值 图 像 采 用 膨胀 操作 ， 并 将 所 产生 的 另 一 幅 二 值 图 像 作为 输出 。 该 
膨胀 操作 可 “加 粗 " 或 “增长 "二 值 图 像 中 的 目标 ， 加 粗 的 形状 可 通过 选择 合适 的 
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e) 

图 3.4 由 无 线 摄像 头 采 集 的 图 像 ( 见 图 3.4a) 、 边 缘 检 测 图 像 ( 见 图 3.4b)、 边 缘 加 粗 

的 图 像 ( 见 图 3.4c) 、 区 域 增长 图 像 ( 见 图 3.4d) 、 包 含 障碍 物 信 息 的 图 像 
( 见 图 3.4e)、 梯 形 地 面 图 像 ( 见 图 3. 4f) 
用 于 执行 图 像 膨胀 的 结构 元 素 的 形状 来 控制 。 在 边缘 图 像 中 连接 并 加 粗 边缘 的 
思想 还 可 通过 选择 合适 的 滤波 掩 码 利用 低 通 滤波 来 实现 。 在 本 系统 中 是 通过 几 
何 均 值 滤 波 来 实现 该 操作 的 。 几 何 均 值 滤 波 器 是 一 种 非 线 性 均值 滤波 器 ， 在 去 
除 高 斯 曝 声 和 保持 边缘 特征 方面 比 算术 均值 滤波 融 更 为 有 效 。 图 3.4c 给 出 了 边 
缘 连 接 和 加 粗 后 的 图 像 。 
5. 对 边缘 加 粗 的 图 像 进行 区 域 增长 分 割 

一 旦 经 过 边缘 加 粗 ， 图 像 就 可 分 割 为 几 个 区 域 , 为 找 出 图 像 中 的 障碍 区 和 
非 障 碍 区 ， 对 边缘 加 粗 的 图 像 执行 基于 区 域 增 长 的 分 割 算法 ， 区域 增长 算法 是 
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一 种 简单 而 有 效 的 基于 区 域 的 图 像 分 割 方法 ， 可 归 类 为 一 种 涉及 初始 种 子 点 选 
择 的 基于 像素 的 图 像 分 割 方法 。 该 分 割 方法 首先 检查 初始 “种 子 点 ”的 邻近 像素 ， 
来 确定 是 否 应 将 该 像素 点 添加 为 种 子 点 。 然 后 ,通过 检查 所 创建 的 每 个 块 的 特 
性 并 根据 某 种 准则 (相似 的 像素 灰 度 值 、 纹 理 等 ) 将 其 与 相 邻 块 合并 。 选 择 图 像 
底部 中 心 附 近 的 像素 点 作为 进行 区 域 增长 的 种 子 点 ， 如 图 3.4c 所 示 的 点 “S o 
现在 ， 沿 着 从 下 到 上 的 所 有 垂直 线 来 扫描 图 像 。 在 地 面 区 域 结 束 的 点 看 作 障 碍 
物 ， 而 在 障碍 物 之 前 的 所 有 区 域 为 自由 区 域 ， 超 过 障碍 物 的 所 有 区 域 看 作 隐 藏 
区 域 。 图 3.4d 中 给 出 的 下 半 部 分 表示 非 障碍 区 (自由 空间 ) ， 上 半 部 分 表示 隐藏 
区 。 接 下 来 ， 用 两 个 区 域 之 间 的 线 来 标记 障碍 区 ， 图 3. 4e 中 包含 了 这 三 种 区 域 。 
不 断 执行 上 述 过 程 并 更 新 障碍 物 信息 。 

6. 将 区 域 增长 图 像 转 化 为 地 面 区 域 

将 以 障碍 物 信 息 更 新 的 整个 增长 区 域 从 像 平 面 转换 到 地 面 区 域 。 为 利用 单个 
摄像 头 来 计算 3D 坐标 系 中 的 距离 ， 在 此 假设 所 有 障碍 物 都 是 与 地 面 接触 的 ， 并 在 
2D 坐标 下 表示 。 图 3.5 表明 在 给 定 摄像 头 高 度 和 俯仰 角 的 条 件 下 ， 像 平面 上 的 任 
意 一 点 如 何 直 接 映射 到 相对 于 摄像 头 位 置 的 地 面 上 '”。 在 此 ， 假 设 机 器 人 /摄像 头 
的 3D 坐标 系 如 图 3.5 所 示 。 假 设 在 任何 时 刻 ， 该 坐标 系 都 与 机 器 人 /摄像 头 的 当 
前 位 置 关 联 。 通 过 这 种 坐标 变换 的 方法 可 在 摄像 头 所 采集 的 图 像 中 确定 自由 点 和 
障碍 点 在 世界 坐标 系 (WCS) 中 的 坐标 。 由 此 ， 根 据 图 3. 5， 像 平面 中 坐标 为 (u,v) 

光学 中 心 点 


虚拟 像 平面 Edi 





图 3.5 图 像 坐标 与 移动 机 器 人 坐标 之 间 的 关系 图 
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的 任意 一 点 都 可 转换 为 机 器 人 /摄像 头 坐 标 系 中 的 二 维 坐标 (*.，y.) ， 即 


uh 
e = (fainOy, + 06080, ) 
h(vsinO,, — fcosQ,, ) 
(vcosQ,, +fsinO,, ) 
式 中 一 从 基 平 面 到 摄像 头 光学 中 心 的 高 度 ; 
/一 摄像 头 的 焦距 ; 
Or —— BRK BE ffs ; 
We ts =A( SEX ) E 








Ye 


—HRHEREM, ÆRE y.) MPLA AM EAr, Yro Pr) 的 条 
件 下 ， 即 可 求 得 地 面 上 点 (x*，7y) 的 实际 位 置 。 图 3. 4f 给 出 了 具有 障碍 信息 的 地 
面 ， 变 换 后 的 地 面 区 域 呈 梯 形状 。 然 后 ， 将 该 地 面 图 像 复 制 到 一 个 500 x 500 像 
素 的 地 图 ( 即 20m x20m) 中 作为 机 器 人 的 工作 空间 。 图 3. 6a 给 出 了 地 图 创建 的 
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a) 
图 3.6 地 图 创建 软件 开发 界面 ( 见 图 3. 6a) 
和 包含 障碍 物 信 息 的 地 图 更 新 界面 ( 见 图 3. 6b) 
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b) 


图 3.6 地 图 创建 软件 开发 界面 ( 见 图 3. 6a) 
和 包含 障碍 物 信息 的 地 图 更 新 界面 ( 见 图 3. 6b)( 续 ) 
界面 ， 图 3. 6b 给 出 了 包含 地 面 图 像 的 栅 格 地 图 。 在 图 3.6b 中 ， 梯 形 地 面 区 域 用 
绿色 表示 ， 障 得 物 信 息 用 红色 表示 。 即 使 在 机 器 人 运动 时 ， 也 不 断 进行 上 述 变 
换 过 程 ， 并 在 地 图 中 更 新 障碍 物 信 息 。 














3.5 最 短路 径 计 算 和 子 目标 生成 


在 移动 机 器 人 导航 过 程 中 ， 确 定 机 器 人 当前 位 置 和 目标 点 之 间 的 最 优 路 径 
是 一 个 重要 目标 ， 这 样 就 能 使 得 机 器 人 在 避 障 的 前 提 下 尽 可 能 短 时 间 到 达 目 的 
地 。 在 此 ， 采 用 一 种 基于 启发 式 的 梯度 方法 根据 栅 格 地 图 来 寻找 最 短路 径 ” 。 
算法 3. 1 中 详细 描述 了 该 算法 。 在 之 前 的 步骤 中 已 知 机 器 人 的 初始 位 置 和 目标 
位 置 以 及 障碍 物 信息 ， 然 后 利用 最 速 下 降 法 来 确定 最 短路 径 上 的 各 点 坐标 。 最 
速 下降 算 法 是 采用 梯度 函数 来 确定 函数 下 降 最 快 的 方向 。 以 特定 步 长 沿 梯度 函 
数 下 降 最 快 方向 迭代 执行 该 算法 ， 然 后 重新 计算 梯度 来 确定 新 的 运动 方向 。 如 
果 地 面 上 的 障 得 物 以 及 初始 点 和 目标 点 已 知 ， 那 么 这 种 局 发 式 方法 是 很 容易 理 
解 的 。 假 设 地 面 无 任何 摩擦 ， 也 就 是 说 ， 如 果 在 地 面 上 的 起 始点 处 倾倒 沙子 ， 
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那么 这 些 沙 子 会 向 所 有 可 能 的 路 径 扩散 ， 这 类 似 于 流体 在 所 有 可 能 方向 上 的 扩 
散 。 显 而 易 见 ， 肯 定 不 会 经 过 障碍 。 在 每 次 迭代 中 ， 假 设 都 倾倒 固定 数量 的 沙 
子 并 任 其 扩散 。 当 沙子 达到 一 定 的 预 设 高 度 时 ， 对 地 面 上 的 每 个 点 都 设置 一 个 
相当 于 迭代 次 数 的 时 间 序 号 。 由 此 ， 越 早 达到 该 高 度 ， 则 时 间 序 号 就 越 小 。 选 
择 该 预 设 高 度 和 固定 数量 的 沙子 以 避免 在 任意 有 限 区 域内 饱和 。 由 此 ， 一 个 行 
程 时 间 和 矩阵 (五 ) 可 通过 有 限 元 扩散 法 来 计算 ,并 迭代 更 新 该 互 矩 阵 ， 直 到 满足 
终止 条 件 。 在 上 述 过 程 结束 时 ,有 HH 矩阵 中 值 为 零 的 元 素 对 应 障碍 单元 。 接 下 来 ， 
采用 基于 梯度 下 降 的 方法 来 确定 从 目标 点 开始 到 机 器 人 当前 位 置 的 最 短路 径 上 
各 点 的 坐标 。 为 此 ,计算 五 在 x Aly Jr 18) EA, MVH, FRIVH,, Jf fi 
据 该 梯度 矩阵 来 计算 最 短路 径 上 下 一 点 的 新 坐标 ， 直 到 求 得 最 短路 径 上 最 后 一 
点 的 坐标 。 该 算法 总 是 从 目标 点 开始 后 向 执行 ， 这 是 一 种 迭代 执行 的 有 效 方法 。 
图 3.7 给 出 了 一 个 计算 出 地 图 中 初始 点 与 目标 点 之 间 最 短路 径 的 示例 环境 . 
算法 3.1 可 实现 避 障 的 最 短路 径 生 成 算法 





开始 
1. 获取 占据 栅 格 矩阵 (M)， 起 始点 (x_start，y_start) 以 及 目标 点 (x_ goal, 
y goal), M(i, j) =0 表示 一 个 自由 栅 格 ，M(i, 7) =1 表示 一 个 障碍 栅 格 < 
2. 创建 扩散 矩阵 (WW) 和 行程 时 间 和 矩阵 (五 ) ， 并 使 这 两 个 矩阵 与 M 和 矩阵 同样 
大 小 。 初 始 化 W, =H, =0。 
3. iX W,(x_start, y start) =1。 
4. 设置 扩散 系数 ( diffconst) Al JC Sr AY c KE LK BL (no. update. max). 9] 
始 化 迭代 次 数 (iter_count) 和 无 更 新 的 迭代 次 数 (no_update_iter_count)。 
5. WHILE(no_update_iter_ count < no_update_ max) 
5.].iter count = iter count + 1, 
5.2. 向 下 扩展 栅 格 : 
Worcs coum Ci gf) = Was (GJ) + diffeonst * Wyo. cum (i +1,/) 
iz1,2,-.,(W ROWS-1);j-21,2,--,W. COLS; 
5.3. 向 上 扩展 栅 格 : 
W, coum ist) = Wer cam (inj) + diffconst * W, cm (i-1,/) 
i=2,3,--,W_ROWS;j=1,2,--,(W_COLS); 
5.4. AD REAR : 
Wi coun tS) = Wi, coun CJ) + diffconst Wi, coum (tJ +1) 
i21,2,-,W ROWS;-1,2,-,W COLS-1; 
5.5. 向 左 扩展 栅 格 : 
Wis uu CJ) = Wi «uu (UJ) + diffconst * Wie. coum (tJ -1) 
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i-1,2,-,W ROWS;j-2,3,-.,W COLS; 
5.6. fi M(i,j) =1, 则 使 得 Wi coun (iJ) 20; 
i-l1,2,-,W ROWS;j-1,2,-.,W COLS; 
5.7. 只 要 存在 Wire .nm(iJ) 首 次 大 于 高 度 , 则 设 相应 的 Hi, quU) = 
iter count, 
5.8. 将 sum count, wn 记 为 当前 WME BAKE 1 的 所 有 元 素 之 和 。 


5.9. IF | sum_ count, | <1,THEN no_update_ 


Mer. count 


— sum. count... count —1) 
iter count = no_update_iter_count +1, 
ENDIF 
ENDWHILE 
6. MAMAS Ai, 站点 为 障碍 点 。 对 这 些 点 设置 较 大 的 值 ， 即 大 于 
邻近 点 中 的 最 大 值 (障碍 点 的 下 降 梯度 )。 
7. 创建 最 短路 径 的 坐标 矢量 sh_path_co ord row 和 sh_path_co ord col, 
并 初始 化 第 一 个 点 为 sh. path. co ord row(1) =x goal; sh path co ord col 
(1) 2y goal, WKH wo 
8. 计算 H JB efe x Zr] EM BRE Ge CV H, ) 和 > 方向 上 的 梯度 矩阵 
(VH,). 
9.VH, = - VH,; VH,= - VH,; path index =1; path flag =1, 
10. WHILE ( path. flag 21) 
10. 1. FF VA, 矩阵 通过 插值 来 计算 del row, 
10.2. AFA VH, 矩阵 通过 插值 来 计算 del col, 
10.3. 计算 最 短路 径 上 下 一 点 的 坐标 : 
sh path co ord row( path index +1) =sh_path_co ord_rom(path_index) + 
del row 
Vdel_row + del. col 
sh path co ord col( path index + 1) - sh. path co ord col( path index) + 


* 








del row 


V del. row + del. col? 
10.4. IF( 达 到 初始 点 )THEN 
path_flag =0; 
ENDIF 
ENDW HILE 
11. 对 矢量 sh. path co ord row 和 sh_path_co ord col 取 反 。 
END 


* 
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一 旦 最 短路 径 确定 后 ， 需 要 寻找 障碍 物 附近 的 角 点 。 为 确定 这 些 角 点 ， 在 
该 路 径 上 取 三 个 连续 点 ， 并 计算 前 两 点 线段 与 后 两 点 线段 之 间 夹 角 的 余弦 。 如 
果 该 余弦 值 小 于 给 定 阔 值 ， 则 将 这 三 个 点 的 中 间 点 作为 角 点 ， 和 否则 移动 到 下 一 
个 点 ， 再 次 计算 新 角度 的 余弦 值 。 不 断 执行 该 过 程 直 到 找到 合适 的 角 点 。 将 该 
角 点 保存 为 导航 的 下 一 个 子 目 标点 。 人 例如， 在 图 3.7 中 ，A1、A2 和 A3 是 所 考 
虑 的 三 个 点 ， 则 线段 A1A2 和 A2A3 夹 角 的 余弦 值 非常 大 (大 于 给 定 阔 值 ) ， 因 此 
A2 不 能 看 作 一 个 角 点 。 然 后 继续 向 前 运动 ， 当 到 达 三 个 连续 点 Bl. B2 和 B3 
时 ,线段 B1B2 和 B2B3 之 间 的 角度 足够 大 使 得 其 余弦 值 小 于 给 定 阐 值 ， 则 将 B2 








图 3.7 某 一 环境 下 最 短路 径 计算 示例 


3.6 第 二 层 中 基于 红外 的 导航 


一 旦 子 目 标点 确定 后 ， 控 制 就 从 第 一 层 转移 到 第 二 层 。 接 收 到 新 的 子 目 标 
信息 后 ， 机 器 人 根据 增 量 编码 器 信息 来 更 新 其 当前 位 置 (xs，ys，q$a )， 并 根据 
当前 位 置 和 子 目标 信息 来 确定 新 的 转向 角 。 理 想 情 况 下 ， 这 就 是 在 静态 场景 中 
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机 器 人 应 旋转 的 角度 ， 然 后 以 恒定 速度 运动 到 子 目 标 ， 这 些 子 目 标 是 在 避 障 前 
提 下 由 最 短路 径 生 成 算法 得 到 的 一 组 点 。 然 而 ， 在 动态 场景 中 ， 激 活 基 于 视觉 
的 地 图 构建 子 程序 后 ， 可 能 新 的 障碍 已 出 现 , 或 原先 障碍 物 的 位 置 已 改变 。 这 
就 可 能 导致 在 沿 机 器 人 和 子 目 标 之 间 的 理想 路 径 运 动 时 会 遇 到 障碍 物 。 为 解决 
动态 环境 下 的 问题 ， 利 用 对 称 安装 在 KOALA 机 器 人 外 围 的 十 六 个 红外 传感器 进 
行 导航 导 引 


这 些 红 外 传感器 在 机 器 人 前 方 分 布 较 密 集 ， 而 在 两 侧 分 布 较 稀 玻 。 图 3. 8a 
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图 3.8 KOALA 机 器 人 的 红外 传感器 分 布 情况 ” 
( 见 图 3.8a) 以 及 在 距离 传感器 ( RO) 前 方 10cm 处 放置 一 个 
1. Sem 宽 的 障碍 物 时 红外 传感器 的 测量 值 ( 见 图 3.8b) 


第 3 章 利用 子 目 标的 基于 视觉 的 移动 机 器 人 导航 55 





给 出 了 移动 机 器 人 的 红外 传感器 分 布 情况 ， 图 3. 8b 给 出 了 在 机 器 人 前 方 10cm 
处 放置 一 个 1.5cm 宽 的 障碍 物 时 红外 传感器 测量 值 的 典型 情况 。 这 十 六 个 红外 
传感器 在 导航 时 对 环境 进行 扫描 。 根 据 这 些 传感器 读数 ， 系 统计 算 机 器 人 前 方 
的 障碍 区 域 和 自由 区 域 ， 并 由 这 些 计算 值 来 确定 机 器 人 的 可 运行 区 域 。 为 确定 
可 运行 区 域 ， 对 这 十 六 个 传感器 每 个 都 分 别 设 置 各 自 的 国 值 ， 并 且 设 传感器 (RO 
~ R3, LO ~L3) 的 优先 级 最 大 。 对 于 每 个 红外 传感器 ， 如 果 读 数 大 于 其 阔 值 ， 则 
意味 着 前 方 有 障 但 ， 耕 则 认为 是 可 运行 的 自由 区 域 。 现 在 假设 , 根据 这 些 读 数 
可 知 在 机 器 人 当前 位 置 的 左 侧 和 右 侧 都 是 可 运行 区 域 ， 这 时 就 应 根据 机 器 人 相 
对 其 理想 运行 方向 转向 最 小 的 方向 来 决定 机 器 人 应 左 转 还 是 右 转 。 一 旦 确定 绕 
行 方向 ， 就 可 根据 该 方向 上 的 红外 传感器 读数 来 确定 机 器 人 的 运动 速度 。 机 器 
人 运行 了 一 段 预 先 设 定 的 距离 后 ， 重 新 激活 红外 扫描 ， 并 重新 确定 运行 的 绕 行 
方向 ,不断 执行 上 述 过 程 直 到 机 器 人 到 达 子 目标 或 其 最 邻近 点 ， 然 后 机 器 人 停 
止 运行 ， 并 重新 返回 到 第 一 层 的 控制 。 


3.7 ”实际 性 能 评估 


在 实验 室 环 境 下 ,利用 几 种 场景 对 基于 视觉 的 导航 进行 性 能 评估 。 在 此 ， 
给 出 四 种 环境 下 的 实验 结果 ， 其 中 在 静态 环境 下 进行 两 次 实验 ， 另 外 两 次 实验 
是 在 动态 环境 下 完成 的 。 

1. 实验 分 析 I 

机 器 人 的 初始 位 置 为 0,，0,，0) ， 目 标点 为 (2，0) 。 在 机 器 人 和 目标 位 置 之 
间 存 在 一 个 障碍 物 。 在 此 需要 注意 的 是 ， 对 于 配置 有 一 个 云 台 系统 以 及 相应 自 
由 度 的 机 器 人 系统 ， 应 在 某 一 特定 环境 下 合理 地 初始 化 水 平角 和 俯仰 角 ， 并 在 
随后 的 所 有 实验 中 保持 不 变 。 初 始 时 ， 对 所 开发 的 系统 选择 这 两 个 角度 使 得 在 
单 目 摄像 头 的 每 帧 图 像 中 都 能 覆盖 相当 大 的 地 面 和 环境 区 域 。 因 此 ， 本 系统 具 
有 一 定 的 灵活 性 ， 可 根据 导航 环境 合理 地 初始 化 这 些 角 。 图 3. 9a 给 出 了 从 摄像 
头 视频 流 中 采集 的 图 像 帧 ， 图 3. 9b ~f 分 别 给 出 了 机 器 人 在 初始 位 置 时 的 图 像 处 
理 步 又 的 顺序 。 机 器 人 在 其 初始 位 置 时 ， 由 机 器 人 所 观察 的 地 面 上 障碍 物 的 边 
缘 实 际 上 是 从 (0.9,，0.3) 到 (0.9，-0.85)。 图 3.10 给 出 了 具有 障碍 物 信息 和 
自由 区 域 的 栅 格 地 图 。 根 据 该 地 图 来 计算 最 短路 径 ， 并 用 子 目 标 信 息 更 新 机 器 
人 导航 算法 的 第 二 层 。 在 此 ， 根据 算法 计算 的 子 目标 1 为 (0.819，0.33)。 在 控 
制 转移 到 第 二 层 后 ， 就 利用 红外 传感器 将 机 器 人 导航 到 子 目 标 1。 这 时 ， 由 于 与 
计算 的 子 目标 有 很 小 的 偏差 ， 机 器 人 实际 停止 在 (0.819,，0.332) 处 。 图 3.11 给 
出 了 机 器 人 达到 第 一 个 子 目标 点 时 的 栅 格 地 图 。 在 控制 又 返回 到 第 一 层 时 构建 
该 栅 格 地 图 ， 并 再 次 执行 基于 视觉 的 处 理 过 程 。 图 3.12a~d 给 出 了 机 器 人 在 第 
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图 3.9 采集 的 图 像 ( 见 图 3.9a) 和 在 第 一 层 执行 的 图 像 处 理 序列 [ 边缘 图 
像 ( 见 图 3.9b) ， 边 缘 加 粗 的 图 像 ( 见 图 3.90) ， 区 域 增长 图 像 
( 见 图 3.9d) ， 包 括 自由 区 域 、 障 碍 区 域 和 隐藏 区 域 的 图 像 
( 见 图 3.9e) 以 及 梯形 地 面 图 像 ( 见 图 3. 9f) ] 
-个 子 目标 点 处 执行 图 像 处 理 步 又 后 的 结果 ， 这 些 结 果 又 用 于 基于 红外 传感器 
的 导航 ， 不 断 重复 上 述 过 程 直到 到 达 最 终 的 目标 点 。 图 3.13 给 出 了 机 器 人 到 达 
最 终 目 的 地 时 的 栅 格 地 图 。 机 器 人 最 终 停 止 在 (1.963，0.024) 处 ,非常 接近 于 
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包括 三 个 区 域 的 初始 地 面 地 图 


— 


机 器 人 起 始 位 置 一 一 





图 3.10 初始 时 的 栅 格 地 图 

指定 的 目标 点 (2,0)。 图 3.14 给 出 了 机 器 人 从 起 始点 开始 在 存在 障碍 物 情况 下 
沿 最 短路 径 到 达 目 标点 所 实际 经 过 的 完整 导航 路 径 。 图 3. 15a 给 出 了 导航 过 程 中 
机 器 人 右 侧 红外 传感器 (RO0，R3 ) 的 响应 曲线 ， 图 3. 15b 给 出 了 机 器 人 左 侧 红外 
传感器 (LO，L3 ) 的 响应 曲线 。 由 图 可 知 ， 当 机 器 人 在 障碍 物 附近 时 ，R3 的 读数 
较 高 。 随 着 机 器 人 绕 过 障碍 物 癌 目标 点 运动 时 ，R3 读数 又 逐渐 减 小 。 

2. 实验 分 析 开 

在 本 实验 中 ， 机 器 人 的 初始 位 置 同样 为 (0, 0,，0)， 新 的 目标 点 为 (3，0) 。 
在 机 器 人 和 目标 位 置 之 间 放置 两 个 障碍 物 。 图 3. 16a 给 出 了 机 器 人 在 起 始 位 置 时 
采集 的 一 帧 图 像 ， 图 3. 16b ~f 给 出 了 在 第 一 层 执行 图 像 处 理 步骤 的 结果 。 图 
3.17 给 出 了 具有 障碍 信息 和 自由 区 域 的 初始 栅 格 地 图 。 接 下 来 ， 计 算 最 短路 径 ， 
并 根据 该 子 目 标 信 息 实现 机 器 人 导航 算法 的 第 二 层 。 图 3. 18 给 出 了 机 器 人 到 达 
第 一 个 子 目标 点 时 的 栅 格 地 图 。 在 机 器 人 到 达 子 目标 1 后 ， 控 制 返回 到 第 一 层 。 
这 时 ， 系 统 再 次 执行 基于 视觉 的 处 理 过 程 ， 如 图 3. 19 所 示 。 图 3. 20 给 出 了 机 器 
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图 3.11 机 器 人 到 达 子 目标 1 时 的 栅 格 地 图 

人 利用 第 二 层 中 的 红外 导航 到 达 子 目标 2 时 的 栅 格 地 图 。 不 断 重 复 执行 该 迭代 
过 程 直到 机 器 人 到 达 最 终 的 目标 点 。 图 3. 21 给 出 了 机 器 人 到 达 最 终 目标 点 时 的 
栅 格 地 图 。 图 3. 22 给 出 了 机 器 人 在 该 静态 环境 中 所 经 过 的 完整 路 径 ， 表 明 机 顺 
人 能 够 圆满 地 到 达 目 标点 。 图 3. 23 给 出 了 在 机 器 人 朝 目 标点 的 导航 过 程 中 ， 四 
个 红外 传感器 RO, R3, LO 和 L3 读数 的 变化 曲线 。 

3. 实验 分 析 王 

在 接 下 来 的 两 个 实验 中 ， 展 示 了 本 书 所 开发 的 系统 在 动态 变化 环境 中 的 应 
用 。 在 此 ， 在 一 个 类 似 于 实验 I 的 环境 中 ,机 器 人 从 起 始 位 置 (0，0，0) 开 始 ， 
在 机 囊 人 和 目标 位 置 之 间 存在 障 但 物 的 情况 下 ， 试 图 到 达 目 标点 (2，0)。 然 而 ， 
在 机 器 人 开始 基于 红外 导航 朝子 目标 1 运动 后 ， 通 过 第 一 层 中 基于 视觉 的 图 像 
处 理 来 确定 机 器 人 起 始 位 置 ， 并 利用 最 短路 径 算法 确定 子 目 标 1 时， 障碍 物 1 的 
位 置 发 生 了 变化 。 新 的 障碍 物 位 置 如 图 3. 24 所 示 ， 这 时 该 障碍 物 运动 到 机 器 人 
附近 ， 并 相对 于 其 初始 位 置 转向 机 器 人 的 左 侧 。 由 于 环境 中 的 动态 变化 ， 机 器 


$33 利用 子 目 标的 基于 视觉 的 移动 机 器 人 导航 59 














c) d) 


图 3.12 在 子 目 标 1 处 的 图 像 处 理 结果 
a) 采 集 的 图 像 b) 边 缘 图 像 “) 边 缘 加 粗 的 图 像 ”d) 包 含 三 个 不 同 区 域 的 图 像 

人 疝 其 左 侧 转 向 ， 但 仍 能 够 避 开 障碍 并 到 达 子 目标 。 随 后 再 次 激活 和 欠 代 算法 ， 
结果 表明 机 器 人 能 够 几乎 完美 地 到 达 其 最 终 目标 。 图 3.24 给 出 了 机 器 人 在 动态 
环境 中 的 导航 。 图 3.25 给 出 了 位 于 机 器 人 前 方 的 红外 传感器 读数 。 由 图 可 看 
出 ， 在 动态 环境 中 有 障碍 物 运动 时 ，R0 和 LO 的 读数 会 产生 突变 。 

在 此 ， 应 注意 的 是 ， 如 果 存 在 动态 变化 的 障碍 物 正 好 运动 到 子 目标 处 的 特 
殊 情况 时 ， 则 根据 该 算法 ,实际 的 红外 导航 机 制 将 能 够 保证 机 器 人 在 满足 避 障 
或 避 碰 的 前 提 下 停止 在 距离 子 目标 最 近 的 位 置 。 

4. 实验 分 析 IV 

本 实验 的 情况 类 似 于 实验 开 ， 只 是 存在 有 自然 运动 的 障碍 物 。 在 此 ， 机 器 
人 开始 朝子 目标 1 运动 后 ， 在 避 开 由 第 一 层 基于 视觉 的 图 像 处 理 确定 的 障 但 物 1 
时 ， 障 得 物 1 的 位 置 突然 变化 。 该 障碍 物 向 机 器 人 左 侧 运动 并 接近 机 器 人 ， 这 
时 在 自由 区 域 中 会 产生 动态 变化 的 区 域 。 同 理 , 在 机 器 人 沿 最 短路 径 运 行 并 导 
开 障 但 物 2 时 ， 障 但 物 2 的 位 置 会 突然 变化 ， 使 得 更 接近 机 器 人 。 然 而 ， 机 器 人 
在 每 种 情况 下 都 能 够 实现 正确 转向 ， 并 圆满 地 到 达 最 终 目 标 ， 如 图 3. 26 所 示 。 
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机 器 人 到 达 最 终 目 标点 时 的 栅 格 地 图 
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图 3.14 机 器 人 所 经 过 的 导航 路 径 
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图 3.15 实验 工 中 红外 传感器 LO 和 L3 随时 间 变 化 的 响应 曲线 
( 见 图 3.15a) 以 及 红外 传感器 RO 和 R3 随时 间 变 化 的 响应 曲线 ( 见 图 3. 15b) 
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e) f) 


图 3.16 采集 的 图 像 ( 见 图 3. 16a) 和 在 第 一 层 中 执行 图 像 处理 步 又 的 结果 

[边缘 图 像 ( 见 图 3.16b)， 边缘 加 粗 的 图 像 ( 见 图 3. 16c) ， 区 域 增长 图 像 

( 见 图 3.16d)， 包含 自 由 区 域 、 障 但 区 域 和 隐藏 区 域 的 图 像 ( 见 图 3. 16e) 
以 及 梯形 地 面 图像 ( 见 图 3. 160) ] 
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PA 3.17 初始 时 的 栅 格 地 图 
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机 器 人 到 达 子 目标 1 时 的 栅 格 地 图 
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图 3.19 在 子 目标 1 处 的 图 像 处 理 结果 
a) 采 集 的 图 像 
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图 3.20 机 器 人 到 达 子 目标 2 时 的 栅 格 地 图 
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图 3.22 实验 1 中 机 器 人 所 经 过 的 导航 路 径 





日 ”此 处 原 书 有 误 ， 应 为 实验 全。 一 一 详 者 注 





目标 点 ， 


66 


基于 视觉 的 自主 机 器 人 导航 





右 侧 传感器 读数 


左 侧 传感器 读数 





时 间 /s 


a) 





时 间 /s 


b) 
图 3.23 在 实验 开 的 导航 过 程 中 红外 传感器 RO 和 R3 随时 间 变 化 


的 响应 曲线 ( 见 图 3. 23a) 以 及 红外 传感器 LO 和 L3 随时 间 
变化 的 响应 曲线 ( 见 图 3. 23b) 
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图 3.24 动态 环境 中 机 器 人 所 经 过 的 导航 路 径 
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图 3.25 在 实验 亚 的 动态 环境 导航 过 程 中 红外 传感器 


RO 和 R3 随时 间 变 化 的 响应 曲线 ( 见 图 3. 25a) 以 及 红外 传 感 
器 LO 和 L3 随时 间 变 化 的 响应 曲线 ( 见 图 3. 25b) 
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图 3.25 在 实验 亚 的 动态 环境 导航 过 程 中 红外 传感器 
RO 和 R3 随时 间 变 化 的 响应 曲线 ( 见 图 3. 25a) 以 及 红外 传 感 
器 LO 和 L3 随时 间 变 化 的 响应 曲线 ( 见 图 3. 25b)( 续 ) 
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图 3.26 动态 环境 中 机 器 人 所 经 过 的 导航 路 径 
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图 3.27 给 出 了 红外 传感器 RO, R3, LO 和 L3 的 读数 。 由 图 可 以 看 出 ， 当 两 个 障 
碍 物 的 位 置 发 生变 化 时 ，L0 和 RO 的 读数 也 产生 两 个 突变 。 
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图 3.27 在 实验 的 动态 环境 导航 过 程 中 红外 传感器 RO 和 R3 随时 间 
变化 的 响应 曲线 ( 见 图 3. 27a) 以 及 红外 传感器 LO 和 L3 随时 间 
变化 的 响应 曲线 ( 见 图 3. 27b) 


70 基于 视觉 的 自主 机 器 人 导航 





3.8 本 章 小 结 


本 章 介 绍 了 如 何 开发 一 个 目标 导向 的 基于 视觉 的 两 层 导 航 机 制 。 该 系统 在 
第 一 层 中 采用 基于 视觉 的 环境 分 析 ， 其 中 利用 几 种 图 像 处 理 函 数 以 及 一 个 最 短 
路 径 生 成 算法 来 确定 导航 的 下 一 个 子 目 标 ， 并 在 避 障 前 提 下 到 达 最 终 目标 。 在 
第 二 层 中 利用 该 子 目 标 信 息 在 动态 环境 中 进行 机 器 人 导航 ， 其 中 利用 一 组 红外 
传感器 来 实现 避 障 并 到 达 子 目标 或 其 邻近 点 。 和 迭代 执行 该 两 层 算法 来 创建 下 一 
个 子 目标 ， 并 将 机 器 人 导航 到 该 目标 ， 从 而 实现 快速 到 达 最 终 目 标 。 本 章 给 出 
了 一 种 如 何 综合 利用 基于 摄像 头 的 图 像 处 理 方法 和 最 短路 径 生 成 算法 来 提高 机 
舌 人 在 实际 环境 中 视觉 导航 效果 的 成 功 实现 ， 从 而 使 得 机 器 人 治 着 实际 最 短路 
径 在 避 障 前 提 下 到 达 其 最 终 目标 (已 知 其 先 验 知 识 ) 。 系 统 的 鲁 棒 性 可 由 基于 红 
外 的 导航 进一步 保证 ， 这 能 有 助 于 机 需 人 在 障碍 物 位置 改 变 的 动态 环境 下 上 自 适 
应 导航 。 最 后 ， 在 实验 室 的 几 种 室内 环境 下 应 用 了 该 算法 。 实 验 结果 表明 KOA- 
LA 机 器 人 可 在 静态 环境 和 动态 环境 下 圆满 地 完成 任务 。 

所 开发 的 程序 包括 了 VB 平台 下 开发 的 能 够 与 机 器 人 系统 的 处 理 器 (下 载 了 
用 户 编 程 的 交叉 编译 C 程序 ) 实 时 通信 的 高 层 程序 。 然 而 ， 在 实际 的 实现 阶段 ， 
整个 系统 可 通过 一 个 人 机 友好 的 GUI 程序 在 PC 的 VB 平台 上 运行 ， 从 而 普通 用 
户 能 够 很 容易 地 应 用 该 系统 。 

对 于 光照 较 好 的 情况 ， 正 如 所 期 望 的 ， 算 法 能 够 提供 比较 满意 的 性 能 。 然 
而 ， 对 于 光照 条 件 较 差 的 情况 ， 地 面 上 障碍 物 的 反射 可 能 较 暗 ( 见 图 3.4a~e)， 
从 而 使 得 边缘 图 像 可 能 对 应 地 面 反射 的 边缘 。 因 此 ， 这 些 反射 可 看 作 障 碍 物 ， 
从 而 使 得 所 计算 的 自由 区 域 缩 小 。 然 而 根据 算法 ， 在 这 些 情况 下 ， 计 算得 到 的 
最 短路 径 可 能 比 实际 最 短路 径 要 长 ， 但 仍 能 够 保证 在 避 障 前 提 下 机 器 人 导航 的 
安全 性 和 和 鲁 棒 性 。 

本 系统 是 在 真实 环境 下 的 机 器 人 位 置 仅 通过 增 量 编码 信息 由 里 程 计 估 计 而 
得 的 情况 下 开发 的 ， 这 在 地 面 均匀 的 室内 环境 下 的 应 用 效果 很 好 。 所 进行 的 实 
验 充分 表明 在 上 述 条 件 下 ， 对 于 不 同 的 环境 配置 机 此 人 可 以 到 达 实 际 环境 中 
的 目标 。 然 而 ， 由 于 车 轮 打滑 等 问题 ,在 室外 环境 中 该 系统 的 准确 性 可 能 会 受 
到 影响 。 在 接 下 来 的 研究 工作 中 应 考虑 如 何 使 得 该 系统 同样 适用 于 室外 环境 ， 
这 可 能 会 通过 在 当前 系统 中 集成 用 于 机 器 人 自 定 位 的 EKF 算法 来 实现 。 

致谢 本章 所 介绍 的 工作 得 到 了 印度 大 学 教育 资助 委员 会 重大 研究 项 目 计 
划 的 资助 [资助 号 : 32-118/2006 (SR) ] . 
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摘要 : 本 章 将 介绍 如 何在 实验 室 环境 下 自行 开发 一 种 具有 特殊 功能 的 低 成 
本 机 器 人 。 尤 其 是 将 详细 介绍 两 种 能 与 机 器 人 集成 的 基于 PIC 微 控制 器 的 传 感 
器 系统 的 开发 过 程 : 其 中 一 种 是 具有 动态 测 距 增强 功能 的 红外 测 距 系统 ; 另 一 
种 是 应 用 开关 模式 同步 检测 技术 的 光学 接近 检测 系统 。 


在 KOALA 机 器 人 (实验 室 购置 的 KOALA 机 器 人 ) 上 实现 基于 视觉 的 导航 算 
AEF, AX KOALA 机 器 人 的 操作 具有 一 定 约束 条 件 ， 具 体 如 下 : 

1) 通过 RS232 实现 PC 笔记 本 电脑 与 KOALA 机 器 人 之 间 的 通信 ， 然 而 现 
今 大 多 数 的 PC/ 笔记 本 电脑 都 已 不 再 配置 串口 ， 因 此 需要 一 个 独立 的 USB 一 串口 
转换 器 来 实现 与 KOALA 机 器 人 之 间 的 操作 。 

2) 在 PC 与 KOALA 机 器 人 之 间 无 法 高 速 传 输 数 据 。 

3) KOALA 机 器 人 的 1/0 接口 有 限 。 

4) KOALA 机 器 人 没有 提供 任何 USB 接口 。 

5) 低 成 本 的 USB 网 络 摄像 头 无 法 与 KOALA 机 器 人 直接 相连 。 

6) KOALA 机 器 人 配置 的 低 端 处 理 器 不 能 进行 图 像 处 理 ， 因 此 需要 一 个 具有 
无 线 摄像 头 接口 的 车 载 笔记 本 电脑 或 PC， 这样 会 使 得 结构 复杂 且 体 积 庞大 。 

因此 ， 本 实验 室 旨 在 自主 研制 开发 一 种 能 够 克服 上 述 缺 点 的 机 器 人 ， 其 具 
体 功 能 和 能 力 将 在 本 章 详细 介绍 。 


4.2 基于 视觉 的 低 成 本 移动 机 器 人 的 研制 开发 


如 上 所 述 ， 为 了 对 工业 行业 提供 一 种 低 成 本 的 解决 方案 ， 本 实验 室 自行 研 
制 了 一 种 移动 机 器 人 。 图 4.1 给 出 了 实际 机 器 人 的 前 视图 和 底 视 图 。 机 器 人 
的 结构 框图 如 图 4.2 所 示 。 所 研制 开发 的 机 器 人 是 一 个 两 轮 的 差 动 驱动 系统 ， 
该 机 器 人 配置 了 六 个 红外 接近 传感器 、 一 个 红外 测 距 系统 和 一 台 笔 记 本 电脑 。 
接近 传感器 提供 了 一 种 布尔 信号 ， 若 机 器 人 与 障碍 物 之 间 的 距离 足够 近 ， 则 每 
个 传感器 均 被 激活 ， 反 之 则 保持 禁用 状态 。 由 于 红外 传感器 系统 的 角 位 置 由 一 
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图 4.1 目 主 开发 的 移动 机 器 人 
a) 前 视图 b) 底 视图 
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4.2 移动 机 器 人 的 原理 示意 图 

个 伺服 电动 机 控制 ， 由 此 对 整个 系统 增加 了 一 个 自由 度 ， 这 样 就 能 使 得 红外 传 
Rar TE F — T fütu EM ASI RS FS BL d A B 7r FILMS MU I FREE. fk BL aS IR EE 
中 配置 一 台 笔 记 本 电脑 ， 从 而 使 其 成 为 一 个 独立 自主 的 系统 。 该 笔记 本 电脑 包 
括 4GB 固态 硬盘、1GB 内 存 和 Windows XP SP2 操作 系统 ， 且 与 任何 运动 部 件 均 
独立 ， 并 通过 USB 与 机 器 人 本 体 进行 通信 机 表 人 本 体 由 笔记 本 电脑 通过 两 根 
USB 线 供电 (5V，1A)， 因 此 移动 机 器 人 的 运行 无 需 独 立 电源 ， 对 所 有 RC 伺服 
电动 机 均 能 通过 电能 控制 来 节能 ， 利 用 霍 尔 效应 开关 来 实现 左 、 右 轮 的 编码 器 
(4 脉冲 Zr) ， 利 用 一 个 具有 上 自动 对 焦 的 笔记 本 电脑 摄像 头 可 作为 机 器 人 系统 的 单 
目 视 觉 传 感 器 另外， 还 专门 开发 了 用 于 障 但 物 检 测 和 避 障 的 红外 测 距 感知 系 
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统 ， 该 系统 采用 一 个 自主 开发 的 微 控制 咒 系 统 (PIC12F683 ) 来 提高 测 距 系统 的 动 
态 范围 。 同 时 ， 该 系统 还 采用 一 个 基于 Visual Basic 的 机 器 人 控制 程序 并 结合 视 
党 和 红外 测 距 传感器 来 实现 机 器 人 导航 功能 . 另外， 该 系统 还 配置 了 WiFi 用 于 
无 线 远程 监测 与 监控 


4.3 ”基于 微 控制 器 的 机 器 人 感知 系统 的 研制 开发 


本 书 所 开发 的 机 器 人 具有 三 种 特殊 功能 。 首 先 ， 该 机 器 人 包括 自主 研发 的 
两 种 特殊 类 型 的 传感器 系统 : 具有 动态 测 距 增强 功能 的 红外 (1IR) 传 感 器 ”; R 
用 开关 模式 同步 检测 技术 的 光学 接近 检测 器 ” 。 这 两 种 传感器 系统 都 是 基于 
PIC 微 控制 器 开发 的 。 此 外 ， 机 融 人 系统 还 具有 内 网 连接 的 复杂 功能 ， 其 中 工作 
在 从 模式 下 的 机 器 人 上 配置 的 笔记 本 电脑 可 由 工作 在 主 模式 下 的 远程 PC 通过 发 
出 合适 的 指令 来 控制 。 


4.3.1 具有 动态 增强 功能 的 红外 测 距 系统 = 


本 书 所 开发 的 机 器 人 系统 配置 有 - -个 具有 动态 测 距 增强 功能 的 自主 开发 的 
基于 PIC 微 控 制 器 的 红外 测 距 系统 ”。 由 于 红外 (IR) 测 距 传 感 器 具有 体积 小 、 
简单 易 用 、 成 本 低 和 功 耗 低 等 特点 ， 因 此 广泛 用 于 机 器 人 的 测 距 应 用 。 在 常规 
形式 下 ， 利 用 三 角 测 量 原理 的 SHARP 红外 测 距 广泛 应 用 于 现实 生活 号 。 在 此 ， 
红外 光 从 物体 反射 的 角度 取决 于 与 物体 之 间 的 距离 。 在 本 书 的 机 器 人 中 ， 所 采 
用 的 红外 测 距 系统 是 基于 散射 线 的 检测 开发 的 ， 在 基于 传统 三 角 测量 原理 的 情 
况 下 ， 可 减少 物体 表面 朝向 对 传感器 读数 的 影响 。 通 常 ， 随 着 与 物体 之 间距 离 
的 减 小 ( 即 物体 越 近 )， 红 外 测 距 系统 的 输出 电压 会 增 大 。 然 而 该 系统 只 能 适用 
于 死 区 范围 之 外 ， 因 为 对 于 死 区 内 的 任何 测 距 值 ， 输 出 电压 会 减 小 而 不 是 增 
| 。 出 现 这 种 情况 的 原因 是 在 死 区 内 ， 传 感 器 接收 不 到 罕 红 外 光束 的 概率 增 
大 。 为 提高 基于 红外 传感器 避 障 方法 的 灵敏 度 ， 在 机 器 人 系统 中 构建 了 一 种 自 
主 开发 的 基于 PIC 微 控制 器 的 红外 测 距 系统 ， 而 不 是 仅 采 用 一 个 简单 的 红外 测 
距 传感器 ”。 在 此 所 开发 的 红外 测 距 系统 是 采用 一 种 可 使 得 脉冲 频率 和 红外 能 
量 传输 持续 时 间 逐 步 减 小 的 阵列 方法 。 目 的 是 通过 利用 红外 传感器 系统 的 输出 
自 适应 地 切换 一 个 红外 LED 的 开 / 关 来 减 小 死 区 范围 。 该 系统 是 利用 在 红外 传 感 
器 两 侧 上 独立 分 布 的 两 个 红外 源 来 实现 测 距 增强 的 。 

图 4.3 给 出 了 采用 PIC12F683 微 控制 器 开发 的 硬件 系统 示意 图 ”。 两 个 高 





4.3.1 节 是 基于 Anjan Rakshit 和 Amitava Chatterjee F 2010 年 10 J] {E IEEE 传感器 期 刊 第 10 期 上 
1635 ~ 1636 页 发 表 的 论文 “A microcontroller based IR range finder system with dynamic range enhane- 
ment ( 一 种 具有 动态 测 距 增强 功能 的 基于 微 控 制 器 的 红外 测 距 系统 )”。 
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图 4.3 ”所 开发 的 用 于 动态 测 距 增强 的 基于 PIC12F683 微 处 理 器 的 红外 测 距 系统 
(在 得 到 IEEE 的 许可 下 ， 从 参考 文献 [2] 中 引用 ) 

强度 的 红外 LED(IR_LED1 和 IR_LED2 ) 发 出 的 红外 能 量 由 一 个 在 图 4.3 中 称 为 
红外 传感器 (IR_Sensor) 的 SHARP 红外 传感器 系统 (IS 1U60) HW, IS 1U60 系 
统 的 内 部 框图 ”表明 当 该 接收 器 接收 输入 的 红外 能 量 时 ， 传 感 器 的 输出 电压 会 
降低 ， 反 之 亦 然 。 带 通 滤波 器 的 中 心 频率 f=38kHz。 出 于 频率 调制 的 目的 ， 载 
波 频率 大 约 为 38kHz 时 的 相对 灵敏 度 最 大 “。 在 标 称 情况 下 ， 可 输出 持续 时 间 
为 600hs 的 占 空 比 为 50% 、 频 率 为 38kHz 的 脉冲 波 信号 o 

算法 4.1 给 出 了 在 PIC 微 控 制 器 上 实现 的 主 程序 。 算 法 4.2 给 出 了 定时 器 1 
(Timerl ) 溢出 使 能 时 所 开发 的 实时 中 断 子 程序 ， 该 子 程序 与 主 程序 共同 执行 。 
在 此 引入 两 个 数组 : 控制 IR_LED 载波 频率 或 脉冲 频率 的 Burst_Freq_Array 和 决 
定 一 次 扫描 中 IR. LED 传输 多 久 的 Inegral _Cycle_Array。 在 传统 的 系统 中 ， 占 空 
比 为 50% 的 脉冲 频率 为 38kHz， 传 输 持 续 时 间 为 600ks ， 并 且 在 范围 相对 较 大 
的 一 个 罕 带 宽 上 ， 传 感 器 可 得 到 一 个 灵敏 结果 。 在 此 专门 控制 这 两 个 变量 ， 以 
使 得 在 与 距离 相关 的 一 次 扫描 中 几 个 脉冲 频率 持续 时 间 内 ，IR_SENSOR 可 接收 
到 一 些 从 物体 反射 回 的 红外 光 能 量 。 对 该 信息 (Range_Count ) 进行 指数 平均 可 产 
生 一 个 稳定 的 PWM 信号 。 对 于 一 个 合理 的 传感器 处 理 速度 ， 只 能 建立 有 限 长 度 
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的 阵列 ， 由 此 对 所 开发 的 系统 产生 “距离 量化 "或 有 限 解 。 
算法 4. 1 PIC 微 控制 器 中 的 主 程序 





开始 

在 关闭 状态 下 初始 化 IR_LED1 和 IR_LED2。 
设置 Burst_Freq_ Array 和 Integral Cycle. Array, 
设置 用 于 定时 器 1 中 断 时 的 定时 器 1 寄存 器 对 。 
编制 适当 的 PWM 载波 频率 。 

从 中 断 子 程序 接收 Range Count 信息 。 

对 该 信息 扩展 ， 以 适用 于 PWM 生成 。 

利用 指数 平均 生成 PWM 信和 号 。 

返回 到 5。 

结束 


9 x AAR YN 





算法 4.2 中 断 子 程序 








开始 
1. 准备 利用 Burst_Freq_ Array [i] 进行 中 断 。 
2. iX Countl max = Internal. Cycle Array [i], 
3. IF( Countl > Count] max), 
切换 Burst Duration flag, Jf 8 ®© Countl 。 
IF( Burst. Duration. flag = 20), 
i JN 1; 
IF(SIGIN = 20), 
jm. 
ENDIF 
ENDIF 
IF (i 达到 Burst_Freq_ Array 中 最 后 一 个 元 素 , 
Range Count-j; 重 置 1 和 j; 
ENDIF 
ENDIF 
4. IF(Burst_Duration_flag = =1), 
若 Burst Freq flag =0， 则 IR. LEDI 打开 。 
Zi Burst_Freq_flag =0 H. SIGIN =0, Wj IR_LED2 打开 。 
ELSE 
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IR_LED1 和 IR. LED2 全 部 关闭 . 
ENDIF 
5. 切换 Burst_Freq_flag。 
6. 清除 Interrupt_ flag。 
结束 





4.3.1.1 动态 测 距 增强 算法 

通过 将 IR_SENSOR 的 输出 作为 微 控 制 器 的 一 个 反馈 信号 (SIGIN ) 来 实现 动 
态 测 距 增 强 ， 即 自 适应 地 开 / 关 IR_LED2。 算 法 4.2 给 出 了 由 Burst_Freq_flag 和 
SIGIN 的 状态 来 控制 IR_LED2 闪烁 。 一 个 常用 的 红外 测 距 仪 中 ， 在 死 区 范围 内 
时 ， 从 物体 反射 回 的 大 多 数 红 外 能 量 都 无 法 由 IR_SENSOR 感知 。 在 本 系统 中 ， 
对 于 距离 较 远 的 物体 ， 大 多 只 有 IR_LED1 闪烁 。 随 着 逐渐 接近 死 区 ，IR_LED2 
开始 激活 ， 这 是 由 于 SIGIN 减 小 的 概率 较 大 。 这 种 以 智能 方式 自 适 应 地 将 IR 
LED2 的 激活 与 距离 减 小 相对 应 ， 将 有 助 于 减 小 死 区 范围 并 实现 所 需 的 动态 测 距 
增强 。 这 与 常用 红外 测 距 仪 的 工作 原理 形成 鲜明 对 比 ， 在 常用 红外 测 距 仪 中 ， 
死 区 范围 内 从 物体 反射 回来 的 大 部 分 红外 能 量 都 无 法 由 IR_Sensor 感知 。 

4.3.1.2 实验 结果 

在 本 实验 室 进行 了 一 个 实验 ， 即 对 于 没有 测 距 增强 的 系统 ， 在 中 断 服务 子 
程序 中 不 利用 反馈 信号 SIGIN 来 控制 IR_LED2。 图 4.4 给 出 了 这 两 种 情况 下 的 
输出 电压 随 距 离 的 变化 曲线 。 在 具有 和 不 具有 测 距 增强 的 两 种 情况 下 ， 在 每 次 
距离 不 同时 ,输出 电压 取 10 次 读数 的 平均 值 。 对 于 没有 动态 测 距 增 强 的 系统 ， 
有 效 距离 为 25 ~50cm， 低 于 25em 就 进入 死 区 。 由 图 可 知 ， 本 书 所 开发 的 系统 
可 减少 死 区 范围 ， 使 得 动态 增强 测 距 的 有 效 距 离 达 到 10 ~ 50cm 


4.3.2 采用 开关 模式 同步 检测 技术 的 光学 接近 检测 器 = 


自主 研制 的 机 器 人 系统 还 配置 了 基于 开关 模式 同步 检测 原理 在 PIC 微 控制 
器 中 开发 的 光学 接近 检测 器 “ 。 近 年 来 ， 基 于 微 控制 器 的 系统 已 广泛 用 于 开发 
低 成 本 的 机 器 人 传感器 系统 ”以 及 一 些 智 能 仪器 仪表 装置 “”。 本 节 将 介绍 一 种 
基于 开关 模式 同步 检测 技术 的 PIC 微 控制 器 ”中 光学 接近 传感器 系统 的 开发 ， 
并 采用 一 种 有 效 策 略 来 提取 受 噪声 严重 影响 的 信号 的 基 波 分 量 。 该 同步 检测 技 
术 已 广泛 应 用 于 AM 无 线 电 接收 器 、 交 流 偏 置 的 应 变 片 桥 式 电路 、 测 温 仪 系 





Q 4.3.2 节 是 基于 Anjan Rakshit 和 Amitava Chatterjee 于 2012 年 5 月 在 测量 科学 与 技术 期 刊 第 3 期 上 
发 表 的 论文 “A microcontroller based compen sated optical proximity detector employing switching-mode 
synchronous detection technique ( 一 种 基于 开关 模式 同步 检测 技术 的 微 控制 器 补偿 的 光学 接近 检测 
tir)” 
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图 4.4 红外 传感器 系统 的 输出 电压 随 距 离 的 变化 曲线 
(在 得 到 IEEE 的 许可 下 ， 从 参考 文献 [2] 中 引用 ) 

统 ”、 机 械 振动 测量 仪 ”、 同 步 相位 电压 转换 器 ”以 及 光纤 传感器 测量 仪 … 等 
装置 中 。 在 此 ， 目 标 是 开发 一 种 成 本 低 但 功能 强大 的 机 器 人 传感器 ， 并 在 环境 
照明 变化 范围 较 大 的 条 件 下 ， 能 够 给 出 与 障碍 之 间 的 准确 距离 。 该 系统 采用 两 
个 发 射 白 光 的 LED 来 确定 与 障碍 之 间 的 距离 。 利 用 一 个 光敏 电阻 (LDR)"” AL 
体 管 组 成 的 电子 电路 来 判断 障碍 物 是 否 足 够 近 。 所 开发 的 系统 具有 外 部 阅 值 变 
化 的 灵活 性 ， 使 得 在 不 同 工 作 环境 下 ， 导 致 传感器 激活 的 最 大 障碍 物 距 离 可 适 
当 变 化 。 同 时 ， 该 系统 还 具有 动态 补偿 功能 ， 使 得 传感器 性 能 几乎 与 环境 的 照 
明 条 件 无 关 。 

影响 接近 传感器 性 能 的 因素 有 很 多 。 一 个 物体 的 检测 本 质 上 取决 于 对 从 物 
体 表面 反射 回 的 辐射 检测 ， 因 此 对 于 物体 与 传感器 之 间 的 距离 相同 ， 则 反射 回 
的 辐射 量 将 取决 于 物体 的 反射 率 。 被 测 物体 的 反射 率 在 0 ~ 1 之 间 变 化 。 高 反射 
率 的 物体 ， 其 反射 率 接近 于 1， 反之 则 接近 于 0。 另 一 个 重要 的 影响 因素 是 物体 
的 表面 光滑 程度 ， 即 白色 LED 光源 的 光 能 量 入 射 物体 表面 的 光滑 度 ( 或 粗糙 
度 ) 。 众 所 周知 ， 如 果 反 射 表 面 足够 大 ， 能 涵盖 两 个 LED 发 射 光 的 整个 空间 分 
布 。 对 于 无 光泽 的 物体 ， 如 果 已 知 物体 表面 的 反射 率 ， 传 感 器 的 模拟 信和 号 可 用 
于 确定 与 物体 的 距离 。 然 而 ， 在 大 多 数 实际 情况 下 ,机 器 人 传感器 并 不 清楚 在 
其 导航 过 程 中 所 将 要 遇 到 的 物体 类 型 ， 因 此 物体 的 反射 率 值 并 不 是 先 验 已 知 的 。 
考虑 到 这 种 情况 ， 所 进行 的 实验 中 设置 了 一 组 反射 率 变化 范围 较 大 的 物体 ， 因 
此 所 开发 传感器 的 适用 性 也 进行 了 广泛 测试 和 验证 。 
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4.3.2.1 基于 PIC 微 控制 器 的 光学 接近 检测 器 
图 4.5 给 出 了 所 开发 的 一 种 基于 PIC12F675 微 控 制 器 的 系统 。 该 系统 具有 与 
两 个 LED 驱动 器 (LED1 和 LED2) 相 连 的 两 路 数字 输出 端 ( 引 脚 3 和 引 脚 5 ) ， 两 
路 模拟 输入 端 ( 引 脚 6 和 引 脚 7) 和 一 路 控制 LED( 记 为 PXD_LED ) 开 / 关 的 数字 
输出 端 ( 引 脚 2) 。 引 脚 7 的 输入 来 自 于 PNP 型 晶体管 的 集 电极 ， 该 晶体 管 发 射 
极 电 路 中 包含 一 个 阻 值 随 光 照 变 化 的 光敏 电阻 (LDR) ”。 在 系统 的 输入 引 脚 6 
处 设计 了 一 个 用 于 调节 阔 值 电压 (THLD_val) 的 外 部 预 设 电 位 器 ， 这 能 够 有 效 防 
Ik PXD_LED 的 误 激 活 。 每 个 LED 都 由 一 个 相同 的 矩形 脉冲 驱动 ， 控 制 其 在 某 
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图 4.5 所 开发 的 光学 接近 检测 器 系统 
a) 便 件 系统 b) 示 意图 
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一 给 定 持续 时 间 内 同时 开 / 关 (ON/OFF)。 

算法 4.3 给 出 了 在 PIC12F675 微 控制 器 上 实现 的 主 程序 。 算 法 4.4 给 出 了 具 
有 Timer! 溢出 使 能 且 与 主 程序 共同 工作 的 实时 中 断 子 程序 。 在 此 ， 所 设计 的 系 
统 能 在 lms 的 时 间 间 隔 内 产生 每 次 中 断 。 每 产生 一 次 中 断 ， 计 数 器 ( 记 为 
Countl ) 值 加 1。 根 据 设 计 思 想 ， 在 驱动 每 个 LED 的 矩形 脉冲 中 ，ON 和 OFF 的 
持续 时 间 均 不 同 ， 在 每 个 周期 中 ，ON 的 持续 时 间 由 参数 Count]. on. max 控制 , 
而 在 每 个 周期 中 ，OFF 的 持续 时 间 由 参数 Count]. off max 控制 。 在 每 个 周期 中 ， 
只 要 Countl 的 值 在 Countl_on_max 范围 内 ， 那 么 LED1 和 LED2 就 都 保持 为 
ON。 对 于 每 个 时 间 周 期 ， 当 Countl 的 值 在 [ Countl on. max, Countl_off_max | 
范围 内 ， 则 LEDI 和 LED2 均 保持 OFF。 当 这 两 个 LED 为 ON 时 ， 发 出 白光 。 根 
据 从 附近 物体 反射 回 的 光 辐 射 量 ， 可 由 LDR 晶体 管 发 出 ， 在 输入 引 脚 7 接收 的 
电压 信和 号 来 确定 与 物体 之 间 的 距离 。 对 于 每 次 采集 到 的 输入 信号 ， 都 要 保持 到 
一 个 开 / 关 时 间 周 期 的 结束 ， 这 样 是 为 了 在 进行 实际 测量 之 前 保持 模拟 信号 稳 
定 。 因 此 ， 都 是 在 (Countl = = (Countl_on_max -2)) 时刻 或 (Countl = = 
( Countl. off. max -2) ) 时 刻 进行 信号 采集 的 。 

对 每 个 采集 信号 进行 三 点 中 值 滤 波 来 消除 杂 散 高 频 分 量 , 尤其 是 可 能 干扰 
原始 信号 的 脉冲 信号 。 对 于 在 引 脚 7 采集 并 经 过 中 值 滤波 的 信号 ， 若 是 在 LED 
ON 时 间 内 采集 的 ， 记 为 LDR_on_val。 同 理 ， 同 样 的 在 引 脚 7 采集 并 经 过 中 值 
滤波 的 信号 ， 若 是 在 LED OFF 时 间 内 采集 的 ， 则 记 为 LDR_off_val。 不 难 理 解 ， 
如 果 存 在 一 个 非常 近 的 物体 /障碍 物 ， 则 LDR_on_val 要 比 LDR_off_val 大 得 多 。 
因此 ， 理 论 上 ，(LDR_on_val-LDR_off_val) 的 值 越 大 意味 着 与 物体 距离 越 近 ， 
若 (LDR_on_val-LDR_offt_val) 大 于 某 一 国 值 ， 那 么 输出 PXD_LED 将 变 为 ON, 
表示 接近 检测 传感器 被 激活 。 然 而 根据 不 同 的 环境 ， 也 可 能 出 现在 阔 值 固定 的 
某 些 情况 下 ， 即 使 物体 并 不 非常 近 ，PXD_LED 也 会 变 为 ON。 因此， 为 避免 出 
现 这 种 误 激 活 ， 用 户 可 利用 外 部 设置 一 个 POT 来 灵活 调节 所 选 阔 值 (采集 ， 中 值 
滤波 处 理 并 记 为 THLD_val)。 由 算法 4.3 可 知 ， 如 果 (LDR_on_val-LDR_off_ 
val) KF THLD_val， 则 可 认为 接近 传感器 与 障碍 物 非常 接近 ， 和 输出 PXD LED 
AE ON, fih OFF, 

该 系统 还 采用 了 一 种 可 动态 应 对 环境 光照 变化 的 智能 补偿 机 制 。 设 计 LDR 
晶体 管 电路 使 得 晶体 管 始终 保持 LDR 的 两 端 电压 几乎 恒定 ， 由 此 在 不 同 的 环境 
光照 条 件 下 ， 确 保 LDR 具有 相同 的 信号 电 平 。 因 此 ， 即 使 在 环境 光照 最 差 和 最 
佳 的 两 种 极端 条 件 下 ， 都 能 保证 信号 电 平 近似 恒定 。 这 也 保证 了 传感器 具有 近 
似 线 性 的 灵敏 度 ， 即 输出 电压 ( 即 引 脚 7 的 输入 电压 ) 的 增 量 变化 与 光学 传感器 
和 障碍 物 之 间 相 对 距离 的 增 量 变化 的 比值 [ 即 (AWYAx) 值 ] 几 乎 恒定 。 这 能 直接 
转换 为 任何 传感器 系统 设计 中 的 一 个 非常 重要 的 特性 ， 即 尽管 存在 其 他 次 要 因 
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素 ( 本 实验 中 是 环境 光照 ) 的 变化 ， 但 检测 器 输出 电压 的 变化 几乎 是 与 主要 被 测 
对 象 (本 实验 中 是 传感器 与 障碍 物 之 间 的 距离 ) 的 变化 保持 一 致 。 
算法 4.3 PIC 微 控 制 器 中 的 主 程序 





开始 
1. 设置 Timerl Jy 1ms 的 定时 器 1 "Pr 
2. IF(Countl = = ( Countl _on_max-2)), 
在 引 脚 7 接收 来 自 LDR 晶体 管 电路 的 输入 信号。 
ENDIF 
3. IF( Countl = = (Countl. on. max-1)), 
在 引 脚 1 对 10 fiz A-D 转换 的 模拟 信号 进行 中 值 滤 波 ， 并 保存 为 LDR_on_ 
val, 
ENDIF 
4. IF( (Countl > = Count] _on_max) &( Count] < Count2_on_max) ) , 
将 引 脚 6 接收 的 THLD 作为 输入 信号 。 
对 引 脚 6 的 10 位 A-D 转换 的 模拟 信号 进行 中 值 滤波 ， 并 保存 为 THLD_val。 
ENDIF 
5. IF( Count = = ( Count] off max-2)), 
在 引 脚 7 接收 来 自 LDR 晶体 管 电路 的 输入 信和 号 。 
ENDIF 
6. IF( Countl = = ( Count] _ off max-1)), 
在 引 脚 7 对 10 位 A-D 转换 的 模拟 信号 进行 中 值 滤波 ， 并 保存 为 LDR_off_ 
val, 
ENDIF 
7. IF( CLDR. on. val-LDR. off val) > THLD val), 
IF (Count2 = = Count2_ max), 
将 Count2 复位 为 0。 
ENDIF 
IF( ( Count2 > ( Count2_ max - 10) ) &( Count2 < Count2_ max) ) , 
PXD LED Jf, 
ENDIF 
ENDIF 
结束 





算法 4.4 中 断 子 程序 
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开始 

1. 设置 Ims 的 定时 器 中 断 。 

2. Countl 加 1。 

3. IF( Count2 < Count2_ max), 
Count2 加 1。 
ENDIF 

4. IF( Countl > Count] _on_ max), 
LED1 Al LED2 关闭 。 
ENDIF 

5. IF( Count] > Countl_ off max), 
将 Countl 重 置 为 0， 
LED1 和 LED2 打开 。 
ENDIF 

结束 





4.3.2.2 开关 模式 同步 检测 技术 

同步 检测 技术 是 一 种 用 于 在 强 噪 声 中 提取 微弱 信号 基 波 分 量 的 常用 信和 号 处 
理 技 术 。 该 技术 广泛 应 用 于 无 线 通 信 、 工 业 环 境 ( 这 里 存在 严重 噪声 污染 或 干扰 
言 号 的 可 能 性 非常 大 ) 等 领域 ， 而 且 该 技术 还 需要 一 个 频率 和 相位 均 已 知 的 参考 
信号 ”。 另 外 ， 同 步 检测 技术 的 一 个 常见 应 用 包括 设计 调幅 信号 超 外 差 接收 机 。 
在 传统 的 同步 检测 方法 中 ,参考 信号 通常 是 一 个 正弦 信号 或 谐 波 信号 。 对 于 这 
种 传统 技术 ,一 种 常用 的 形式 是 以 方 波 /和 矩形 波 为 参考 信号 的 开关 模式 同步 检测 
(SMSD ) 技 术 。 开 关 模 式 同步 检测 器 的 核心 在 于 采用 了 一 个 相 敏 检测 器 。 在 开关 
模式 同步 检测 技术 中， 一 个 周期 性 矩形 脉冲 序列 作为 参考 信号 TOO. MFR 
样品 声 信 号 x(t) ， 并 对 检测 器 的 输出 信号 x, CO) 进行 低 通 滤波 来 恢复 x (0) 的 基 
波 分 量 ， 即 x.(1t)。 在 本 实验 中 ， 采 用 一 种 改进 的 开关 模式 同步 检测 技术 ， 如 图 
4.6 ia. AP, BBB Aas r( 纹 用 于 采样 噪声 信号 *(1) ， 这 将 产生 一 种 与 传 
统 SMSD 方法 相同 的 检测 器 输出 信号 x, (1)。 然 后 ， 只 有 当 输 出 信号 x, (i) 长 时 
间 产 生 一 个 高 电 平 信号 ， 才 能 激活 LED 的 输出 。 这 与 条 件 采样 和 保持 操作 相 类 
似 ， 可 等 效 于 一 个 低 通 滤波 的 作用 ， 这 可 避免 在 微 控制 器 输入 引 脚 7 采集 的 可 
能 由 一 些 不 必要 的 外 部 干扰 所 产生 的 高 电 平 输入 脉冲 信号 或 短 时 输入 信号 导致 
激活 接近 探测 器 。 如 果 该 信号 产生 一 个 持续 很 长 时 间 的 高 电 平 ， 则 可 推断 这 一 
定 是 由 于 在 传感器 附近 存在 物体 造成 的 ， 而 不 是 采集 的 噪声 信号 。 图 4.7 给 出 
了 在 传感器 附近 存在 物体 时 的 一 个 实际 采样 情况 。 其 中 ， 微 控制 器 引 脚 7 的 采 
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混 频 器 








开关 模式 检测 器 低 通 滤波 器 


图 4.6 一 种 改进 的 开关 模式 同步 检测 器 
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图 4.7 实际 采样 的 输入 信号 x(1)( 见 图 4.7a) 和 参考 信号 r(1)( 见 图 4.7b) 
集 信号 作为 输入 信号 *(t)， 参 考 信 号 表示 为 r(t)。 值 得 注意 的 是 ， 在 开关 模式 
同步 检测 技术 中 ， 要 分 析 的 信号 与 参考 信号 之 间 的 相对 相位 差 具 有 重要 作用 ”。 
对 于 与 参考 信号 相位 不 匹配 的 信号 ， 若 相位 差 为 90*， 则 输出 信号 减 小 ， 且 在 一 
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个 给 定 频率 下 ， 对 开关 模式 检测 顺 输 出 信号 没有 影响 。 在 本 实验 中 ， 在 引 脚 7 
采集 的 晶体 管 发 射 信号 为 x(t) ， 而 LED 产生 参考 信号 r(1) 。 若 一 个 物体 距离 足 
够 近 ， 则 低 通 滤波 器 产生 一 个 较 高 的 输出 信号 ， 市 对 于 距离 较 远 的 物体 ， 则 输 
出 信号 较 低 。THLD 信号 作为 一 个 布尔 信和 号， 用 于 判断 与 物体 之 间 的 距离 。 

4.3.2.3 实验 结果 

利用 所 设计 的 光学 接近 检测 器 在 实际 环境 中 的 各 种 实验 条 件 下 来 检测 周围 
物体 。 每 次 实验 ， 传 感 器 系统 都 能 通过 一 个 布尔 型 输出 ， 即 LED 的 输出 (在 足够 
接近 一 个 物体 时 ，PXD_LED 打开 ， AWRA), 来 表明 具有 良好 性 能 。 然 而 如 
上 所 述 ， 如 果 传 感 器 与 物体 之 间 的 相对 距离 不 断 减 小 ， 则 能 改变 布尔 型 输出 ( 即 
从 关闭 状态 变 为 打开 状态 ) 的 传感器 与 物体 所 处 位 置 之 间 的 最 小 距离 取决 于 多 种 
因素 。 图 4.8 给 出 了 在 测试 中 与 物体 之 间 最 小 距离 变化 的 影响 的 实验 结果 ， 在 
此 需要 一 个 利用 POT 进行 外 部 调节 的 阔 值 电压 (THLD_val) 的 苑 数 来 激活 接近 检 
测 器 。 正 如 预期 的 那样 ， 随 着 阔 值 增 大 ， 一 般 情 况 下 ， 检 测 顺 在 最 小 接近 上 距离 
更 小 时 才 会 激活 。 若 阔 值 设 定 得 较 高 ， 系 统 会 表现 出 一 种 近似 饱和 效应 ， 这 表 
明 对 于 激活 检测 器 的 最 小 距离 存在 一 个 有 效 死 区 。 

图 4.8 中 给 出 了 三 种 不 同类 型 物体 时 的 实验 结果 : 具有 较 高 的 反射 率 (p 
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图 4.8 物体 反射 率 分 别 为 p=33% (a), p= 16% 
(b) 和 p=7.8%(c) 时 接近 检测 器 的 性 能 曲线 
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= 33%); @ 具 有 适中 的 反射 率 (p =16% ); @@ 具 有 和 较 低 的 反射 率 (p =7.8% )。 
这 些 反 射 率 值 都 是 在 波长 为 550nm 处 获得 的 。 利 用 KYORITSU 5200 型 照度 计 ， 
对 每 个 物体 的 反射 率 进 行 实验 测定 。 通 过 保持 每 个 物体 的 反射 表面 与 LED 光 轴 
正 交 来 得 到 实验 结果 - 不 难看 出 ， 对 于 高 反射 率 的 物体 ,在 设置 某 一 给 定 阔 值 
电压 条 件 下 ,距离 相对 较 远 时 即 可 激活 接近 检测 器 。 如 果 换 作 有 具有 低 反 射 率 的 
其 他 物体 ， 同 样 的 冰 值 电压 条 件 下 ， 只 能 在 距离 非常 近 时 才能 激活 接近 传感器 ， 
即 需要 能 够 激活 接近 输出 LED 的 最 短 距 离 非常 小 。 对 于 反射 率 较 低 的 物体 ， 相 
应 的 接近 距离 也 非常 小 ， 且 即使 所 选 的 阔 值 电压 很 小 ， 传 感 器 也 很 快 进 入 死 区 ， 
例如 ， 本 实验 中 ， 在 阐 值 电压 为 0.6V 时 ， 可 进入 物体 (ce) 的 死 区 ， 若 进一步 增 
大 该 浆 值 电压 ， 系 统 就 不 能 有 效 地 接近 检测 。 因 此 ， 为 有 效 利 用 机 器 人 导航 中 
的 接近 传感器 ， 物 体 应 至 少 具 有 中 等 或 中 低 等 的 反射 率 ， 才 能 使 得 机 器 人 在 传 
感 器 激活 的 条 件 下 安全 避 障 。 同 时， 实验 也 验证 了 该 传感器 系统 可 有 效 地 用 于 
检测 最 小 为 6cm x8em 以 及 更 大 的 物体 。 


4.4 客户 端 一 服务 器 工作 模式 下 的 内 网 连接 


除了 上 述 两 种 特殊 类 型 的 传感器 系统 外 ， 自 主 研制 开发 的 机 器 人 还 能 够 在 
机 器 人 上 安装 的 笔记 本 电脑 与 远 端 PC 之 间 进 行内 网 连接 数据 通信 和 控制 命令 交 
互 。 在 这 种 客户 端 一 服务 器 工作 模式 下 ， 机 器 人 作为 服务 器 ， 而 远 端 用 户 作 为 
客户 端 ， 通 过 Windows 环境 下 基于 TCP / IP 的 Socket 编程 来 进行 通信 。 在 客户 
端 能 够 选择 机 器 人 的 功能 ， 如 是 否 在 导航 过 程 中 使 用 接近 传 感 带 ; 是 否 开启 或 
关闭 搜索 模式 。 如 果 开 局 搜索 模式 ， 则 在 机 器 人 偏离 路 径 时 ， 可 利用 所 保存 的 
绕 行 转向 角 信 息 来 导 引 机 器 人 返回 路 径 。 如 果 在 客户 端 取消 搜索 模式 ， 则 机 器 
人 一 旦 偏离 路 径 就 会 停止 运动 。 如 果 需 要 的 话 ， 可 在 远程 操作 中 增加 更 多 控制 
灵活 性 来 实现 更 多 功能 。 服 务 器 也 可 根据 来 自 客 户 端的 “数据 发 送 请 求 ( data 
send. request) "来 发 送 文 本 和 图 像 数 据 。 通 常 ， 由 于 图 像 数据 量 较 大 ， 在 接收 到 
一 个 接收 请 求 时 ， 服 务 器 首先 创建 一 个 用 于 传输 图 像 矩 阵 的 所 有 像素 值 的 数组 。 
然而 ， 整 个 数据 数组 并 不 是 以 连续 方式 传输 ， 而 是 通过 由 低 端 设备 驱动 程序 管 
理 的 一 系列 数据 包 发 送 。 同 时 ， 客 户 端 系统 也 以 同样 的 方式 编程 ， 即 不 断 接 收 
数据 包 ， 直 到 接收 完 一 个 完整 的 图 像 数据 阵列 ， 然 后 重建 在 客户 端 显示 的 图 像 。 
该 系统 具有 互 锁 功能 ， 即 当 客 户 端正 在 处 理 上 一 个 接收 数据 包 的 请 求 过程 中 ， 
不 允许 发 送 新 的 数据 请 求 。 图 4. 9 分 别 给 出 了 显示 一 帧 采集 图 像 和 在 服务 器 对 
该 图 像 进行 路 径 提取 的 客户 端 用 户 界面 。 根 据 图 形 用 户 界面 的 显示 以 及 前 面 所 
述 ， 通 过 在 客户 端点 击 “ 接 近 检 测 器 开 / 关 ”按钮 来 激活 或 停 用 红外 接近 传 感 
器 ， 该 系统 具有 很 大 的 灵活 性 。 男 外 ， 还 可 通过 点 击 “ 搜 索 模式 开启 /关闭 ” 按 
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图 4.9 机 器 人 导航 过 程 中 在 客户 端 显示 的 一 个 采样 时 刻 时 的 GUI 界面 
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钮 来 表明 在 机 器 人 视 场 中 路 径 消 失 时 ， 机 器 人 是 继续 以 迭代 方式 来 重新 定位 到 
路 径 ， 还 是 只 是 简单 地 停止 进一步 导航 。 


4.5 本 章 小 结 


本 章 介 绍 了 如 何在 实验 室 中 自主 开发 一 个 具有 特殊 功能 的 低 成 本 机 器 人 。 
该 机 器 人 系统 包括 两 种 专门 开发 的 基于 微 控 制 器 的 传感器 系统 ， 并 且 通 过 内 网 
灵活 连接 。 在 两 种 专门 开发 的 传感器 系统 中 ， 基 于 PIC 微 控制 器 的 红外 测 距 系 
统 是 利用 红外 传感器 的 输出 来 自 适 应 地 切换 一 个 红外 LED 的 开 / 关 ， 从 而 实现 动 
态 测 距 增 强 。 该 系统 采用 一 种 基于 阵列 的 方法 来 控制 脉冲 频率 和 红外 能 量 传输 
的 持续 时 间 ， 以 提高 测 距 精度 。 另 一 种 基于 微 控制 器 的 传感器 系统 包括 一 个 由 
发 射 白光 的 LED, IH LDR 和 晶体 管 组 成 的 电子 电路 以 及 一 个 布尔 表示 开 / 关 的 
输出 LED 组 成 的 光学 接近 传感器 检测 系统 。 该 系统 是 采用 开关 模式 同步 检测 技 
术 来 开发 的 ， 同 时 为 了 使 得 该 电路 能 在 不 同 工 作 条 件 下 可 靠 运 行 ， 系 统 中 还 配 
置 了 一 个 调节 外 部 阅 值 的 装置 ， 从 而 便于 用 户 对 系统 性 能 进行 微调 。 最 后 ， 该 
机 器 人 在 客户 端 一 服务 器 工作 模式 下 进行 内 网 连接 ， 其 中 机 侨 人 上 的 笔记 本 电 
脑 工 作 在 从 模式 ， 而 工作 在 主 模式 下 的 远程 PC 可 从 和 远 端 给 机 器 人 发 出 合适 的 操 
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$53 SET GE HY pe s pss 
人 导航 算法 的 实现 示例 


摘要 : APRS Æ 32 位 的 Windows 系统 环境 下 ， 如 何 逐 步 详 细 地 真正 实 
现 基 于 视觉 的 导航 模块 。 在 这 些 示 例 中 ， 首 先是 一 个 从 正在 运行 的 视频 中 采集 
图 像 帧 的 简单 示例 ， 然 后 是 逐步 处 理 集成 了 图 像 处 理 功 能 (如 过 滤 技 术 、 对 比 
度 增强 、 自 适应 阅 值 等 ) 的 复杂 任务 。 另 外 ， 这 些 示 例 还 展示 了 如 何 从 这 些 图 
像 中 提取 机 器 人 路 径 以 及 如 何 采 用 基于 规则 的 方法 来 确定 一 个 差 动 驱动 系统 的 
左右 轮 速度 的 设 定 。 


本 章 一 步 步 地 介绍 了 基于 Visual Basic 的 软件 编程 ， 并 给 出 了 在 PC 中 十 个 
视觉 导航 的 编程 示例 。 在 此 ， 利 用 低 成 本 的 网 络 摄像 头 来 采集 视频 流 ， 并 采用 
Visual Basic 6 ( VB6) ”来 进行 Windows 编程 。 

第 一 个 示例 “示例 1” 介 绍 了 如 何在 低 成 本 网 络 摄像 头 获 取 的 视频 流 中 采集 
图 像 帧 并 用 鼠标 查看 像素 值 。 另 外 还 以 全 像素 方式 进行 RGB (£r—2&— Wk) 到 
灰 度 的 图 像 变换 。 通 过 “Format” 菜 单 来 选择 图 像 帧 大 小 为 160 x120。 调 用 32 
位 Windows API (应 用 程序 界面 ) ”来 加 速 图 像 处 理 过 程 。 

第 二 个 示例 “示例 2” 展 示 了 如 何 处理 从 视频 流 中 采集 的 图 像 帧 。 包 括 
RGB 到 灰 度 的 图 像 变换 和 低 通 滤波 两 种 操作 ” 。 

第 三 个 示例 “示例 3” 给 出 了 在 光照 较 差 的 条 件 下 基于 直方 图 拉 伸 技 术 ”“ 
的 对 比 度 增强 方法 。 

第 四 个 示例 “示例 4” 引 入 了 几何 均值 滤波 器 来 平滑 和 抑制 图 像 细 节 ， 从 而 
简化 了 导航 中 所 需 白色 路 径 的 提取 。 

第 五 个 示例 “示例 5” 在 光照 变化 条 件 下 采用 一 种 自 适 应 国 值 方法 来 提取 白 
色 路 径 。 其 中 ， 一 个 可 选 的 参考 像素 决定 了 所 提取 路 径 的 中 心 。 

第 六 个 示例 “示例 6” 介 绍 了 一 种 清除 操作 来 去 除 立 值 操 作 中 所 检测 到 的 不 
必要 物体 。 

第 七 个 示例 “示例 7” 介 绍 了 一 种 选择 黑色 路 径 或 白色 路 径 的 操作 。 对 于 黑 
色 路 径 ， 首 先 对 灰 度 图 像 帧 反 相 ， 从 而 使 黑色 物体 变 成 白色 ， 然 后 按照 第 六 个 
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示例 中 所 讨论 的 方法 进行 处 理 。 

第 八 个 示例 “示例 8” 目 的 是 以 一 个 固定 参考 像素 点 来 确定 导航 所 需 的 黑色 
或 白色 路 径 。 

第 九 个 示例 “示例 9” 介 绍 了 一 种 基于 规则 的 方法 来 确定 一 个 差 动 驱动 系统 
在 导航 过 程 中 的 左右 轮 速 度 。 在 此 ， 利 用 一 个 合适 的 图 像 文件 来 图 形 化 表示 导 
航 方向 。 

最 后 的 第 十 个 示例 “示例 10” 通 过 增加 声音 输出 来 在 导航 过 程 中 引起 注意 。 

适用 于 Visual Basic 6 和 Visual Basic. net 2010 编译 器 的 源 代 码 可 从 http: // 
extras. springer. com 下 载 。 

若 读 者 没有 相应 的 编译 器 ， 本 书 还 提供 了 可 执行 程序 以 用 于 测试 程序 性 能 。 
只 需 从 微软 公司 免费 下 载 所 需 的 运行 时 可 执行 文件 即 可 。 


5.2 示例 1 


目标 : 开发 一 个 采集 网 络 摄像 尖 视 频 流 的 VB6 程序 。 

以 下 步骤 总 结 概括 了 程序 的 开发 过 程 。 

1) 在 “Webcaml. bas” 模 块 中 声明 所 有 必需 的 API 调用 。 需 在 VB6 程序 的 
“Forml ”中 包含 这 一 模块 。 

2) AVICAP32. DLL 用 于 通过 调用 适当 的 API 来 采集 网 络 摄像 头 的 视频 流 。 
网 络 摄像 头 的 视频 格式 应 为 RGB24 或 YUY2。 

3) Æ FORMI 中 添加 两 个 “Picture Box "控件 ，“Picturel ” 用 于 预览 30 iis 
的 视频 流 ,， “Picture2 ”用 于 在 “Timerl ”控件 下 ， 以 10ms 的 固定 间隔 从 视频 流 
中 采集 图 像 ， 并 将 其 作为 剪贴 板 数据 。 

4) 在 “Formnl” 中 添加 “Capture” 和 “Close” 两 个 命令 按钮 用 于 控制 图 像 
采集 过 程 。 这 两 个 命令 按钮 分 别 命名 为 “cmdCapture” 和 “cmdClose " 。 

5) 在 “Forml” 中 添加 菜单 项 “Format” 用 于 设置 图 像 大 小 为 160 x 120 像 
素 。 

6) 通过 鼠标 查看 “Picture2 ”图 像 上 的 所 有 像素 点 。 鼠 标 光 标 会 变 为 十 字形 
以 便于 查看 像素 点 。 

7) 调用 “GetPixel”API 可 获得 像素 点 的 颜色 。 

8) 通过 调用 “GetRed” 、“'GetGreen ”和 “GetBlue” 三 个 图 数 可 从 “Color”， 
中 获得 红 (R), 5& (G) AK (B) 三 个 值 。 三 个 函数 如 下 : GetRed = Color 
And 255, GetGreen = (Color And 65280) 1256 和 GetBlue = ( Color And 16711680) 
165535, 

9) 添加 三 个 命名 为 “Textl ”、“Text2” 和 “Text3” 的 文本 框 来 显示 所 选 像 
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素 点 的 红 (R)、 绿 (G) ME (B) 三 个 8bit 值 。 
10) 两 个 命名 为 “Text4” 和 “Texts” 的 文本 框 用 于 监视 所 选 像 素 点 的 


ex i 


“Y ”上 坐标。 


11) 添加 “Text6， 文 本 框 来 观察 所 选 像素 点 由 RGB 通过 公式 gray = 0. 2125 
* red +0. 7154 * green +0. 0721 * blue 转换 为 8bit 灰 度 值 。 


12) 添加 第 二 个 计数 器 “Timer2 ”控件 可 在 鼠标 指针 不 在 “Picture2 


框 上 时 ，10ms 内 清除 文本 框 中 的 数据 。 


下 面 给 出 “Webcaml. bas” 模 块 的 程序 : 

Global Const WS_CHILD As Long = &H40000000 

Global Const WS_ VISIBLE As Long = &H10000000 

Global Const WM_ USER = 1024 

Global Const WM_CAP_ DRIVER_ CONNECT = WM_ USER +10 
Global Const WM CAP. SET. PREVIEW = WM. USER +50 
Global Const WM_CAP_ SET_ PREVIEWRATE = WM_ USER +52 


图 像 


Global Const WM CAP DRIVER_ DISCONNECT As Long = WM USER +11 
Global Const WM CAP DLG  VIDEOFORMAT As Long = WM_ USER +41 


Global Const WM. CAP GET FRAME As Long = 1084 

Global Const WM CAP | COPY As Long = 1054 

Global Const WM CAP SET SCALE As Integer = WM_ USER +53 
Global Const SWP_NOMOVE As Integer = 2 

Global Const SWP_NOZORDER As Integer =4 

Global Const HWND_ BOTTOM As Integer = 1 





Declare Function SendMessage Lib " user32 " Alias " SendMessageA "_ 
(ByVal hwnd As Long, ByVal wMsg As Long, ByVal wParam As — 
Long, ByVal IParam As Long) As Long 

Declare Function capCreateCaptureWindow Lib " avicap32. dll " Alias - 

" capCreateCaptureWindowA "( ByVal a As String, ByVal b As Long, _ 
ByVal C As Integer, B yVal d As Integer, ByVal e As Integer, _ 
ByVal f As Integer, ByVal g As Long, ByVal h As Integer) As Long 

Declare Function SetWindowPos Lib " user32 "( ByVal hwnd AS Long, — 
ByVal hWndlnsertAfter As Long, ByVal x As Long, ByVal V AS Long, _ 
ByVal cx As Long, ByVal cy As Long, ByVal wFlags As Long) As Long 

Declare Function GetPixel Lib " gdi32 "( ByVal hdc As Long, — 

ByVal x As Long. ByVal y As Long) As Long 
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图 5.1 给 出 了 “Forml ' 的 布局 。 
[ 窗 体 ! | | 图 像框 !| | 菜单 | | 
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图 5.1 “Forml' 的 布局 
以 下 文本 显示 了 “Forml 的 代码 程序 : 
Dim hwdc As Long 
Dim startcap As Boolean 


Dim mflag As Boolean 


Private Sub cmdCapture_ Click ( ) 
hwdc = capCreateCaptureWindow( " Webcam Vision System ", WS. CHILD _ 
Or WS VISIBLE ,0 ,0 ,160,120, Picture 1. hwnd ,0 ) 
If fhwde < >0) Then 
Clipboard. Clear 
If SendMessage(hwdc, WM CAP DRIVER CONNECT,0,0) Then 
SendMessage hwde. WM CAP SET SCALE,True,O 
SendMessage hwdc, WM CAP SET PREVIEWRATE ,30 ,0 
SendMessage hwdc, WM CAP SET PREVIEW 1,0 
SetWindowPos hwdc, HWND BOTTOM,0,0,160,120, S WP NOMOVE _ 
Or SWP NOZORDER 
startcap = True 
cmdCapture. Enabled = Fal se 
cmdClose. Enabled = True 


Timer 1. Enabled = True 
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Menu 1. Enabled = TrHe 
Picture2. Visible = True 
Labell. Visible = True 
Label2. Visible = True 
Label3. Visible = True 
Label4. Visible = True 
Label5. Visible = True 
Label6. Visible = True 
Label7. Visible = True 
Label9. Visible = True 
Label 11. Visible = True 
Textl. Visible = True 
Text2. Visible = True 
Text3. Visible = True 
Text4. Visible = True 
Text5. Visible = True 
Text6. Visible = True 
Else 
MsgBox(" No Webcam found! ") 
startcap = False 
End If 
End If 
End Sub 


Private Sub cmdClose Click( ) 

If startcap = True Then 
SendMessage hwdc, WM CAP DRIVER. DISCONNECT,0. 0 
startcap = False 
cmdCapture. Enabled = True 
cmdClose. Enabled - False 
Timerl. Enabled = False 
Menul. Enabled - False 
Picture2. Visible = False 
Labell. Visible = False 
Label2. Visible = False 
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Label3. Visible = False 
Label4. Visible = False 
Label5. Visible = False 
Label6. Visible = False 
Label7. Visible = False 
Label9. Visible = False 
Labelll. Visible = False 
Textl. Visible = False 
Text2. Visible = False 
Text3. Visible = False 
Text4. Visible = False 
Text5. Visible = False 
Text6. Visible = False 

End If 

End Sub 
Private Sub Form Load( ) 

If ADp. PrevInstance = True Then End ' 不 允许 多 个 实例 

cmdCapture. Enabled = True 

cmdClose. Enabled = False 

Picturel. AutoSize = True 

Picture2. AutoSize = True 

Timerl. Interval = 10 

Timer2. Interval = 10 

Menul. Enabled = False 


mflag = False 
Picture2. Visible = False 
Picture2. MousePointer = 2 ' 十 字 光 标 


Labell. Visible = False 
Label2. Visible = False 
Label3. Visible = False 
Label4. Visible = False 
Label5. Visible = False 
Label6. Visible = False 
Label7. Visible = False 
Label9. Vi sible = False 
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Labelll. Visible = False 
Textl. Visible = False 
Text2. Visible = False 
Text3. Visible = False 
Text4. Visible = False 
Text5. Visible = False 
Text6. Visible = False 
End Sub 


Private Function GetRed( ByVal Color As Long) 
GetRed = Color And 255 


End Function 


Private Function GetGreen( ByVal Color As Long) 
GetGreen = ( Color And 65280) 1256 
End Function 


Private Function GetBlue( ByVal Color As Long) 
GetBlue = (Color And 16711680) 165535 


End Function 


Private Sub Form_MouseMove( Button As Integer, Shift As Integer, _ 

x As Single,y As Single) 

mflag = False ' 鼠标 指针 在 界面 中 ,但 不 在 图 形 框 中 
End Sub 
Private Sub Menul. Click( ) 

If startcap = True Then 

SendMessage hwdc, WM CAP DLG VIDEOFORMAT,0,0 

End If 

End Sub 


Private Sub Picture2_MouseMove( Button As Integer, Shift As Integer, _ 
x As Single,y As Single) 
Dim Color As Long 
Dim red As Byte 
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Dim blue As Byte 
Dim green As Byte 
Dim gray As Byte 
Dim xp As Long 
Dim yp As Long 


xp = x/Screen. TwipsPerPixelX 

yp = y/Screen. TwipsPerPixelY 

Color = GetPixel( Picture2. hdc , xp, yp) 
red = GetRed ( Color) 

green = GetGreen( Color) 

blue = GetBlue( Color) 

gray 20. 2125 x red +0. 7154 * green + 0. 0721 * blue 
Textl. Text = red 

Text2. Text = green 

Text3. Text = blue 

Text4. Text = xp 

Text5. Text = yp 

Text6. Text = gray 


mflag = True ' 鼠 标 指针 在 图 形 框 中 
End Sub 


Private Sub Timerl Timer( ) 


SendMessage hwde, WM CAP GET FRAME,0,0 
SendMessage hwdc, WM CAP COPY ,0,0 

Picture2. Picture = Clipboard. GetData 

SendMessage hwde. WM CAP SET PREVIEW,1,0 


End Sub 


Private Sub Timer2_ Timer( ) 
If mflag = False Then ' 鼠 标 指 针 不 在 图 形 框 中 


Textl. Text = "" 
Text2. Text 2 "" 
Text3. Text = "" 
Text4. Text = "" 
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Text5. Text = "" 
Text6. Text = "" 
End If 
End Sub 
为 执行 程序 ， 需 点 击 采 集 按钮 。 只 要 有 网 络 摄像 头 ， 那 么 就 可 在 图 像框 
'Picturel' ”中 预览 图 像 。 如 果 所 采集 图 像 的 大 小 与 图 像框 “Picture2 ”不 符 ， 则 
需 通过 激活 “Format” 菜 单 来 将 图 像 大 小 转换 为 160 x 120 RA. 
如 果 没 有 可 用 的 网 络 摄像 头 ， 则 会 在 消息 框 中 弹出 消息 “ 找 不 到 任何 网 络 
BERGE! (No webcam found!)”。 


5.3 示例 2 


HER: 开发 一 个 VB6 程序 来 采集 和 处 理 网 络 摄像 头 的 视频 流 ， 将 其 转换 为 
灰 度 图 像 并 进行 低 通 图 像 滤波 。 

以 下 步骤 总 结 概括 了 程序 开发 的 过 程 : 

1) 在 “Webcam2. bas” 模 块 中 声明 所 有 必需 的 API。 另 外 ， 还 需 在 VB6 程 
序 的 “Forml ”中 包含 该 模块 。 

2) AVICAP32. DLL 用 于 通过 适当 的 API 来 采集 网 络 视频 流 . 网 络 摄像 头 的 
视频 格式 应 为 RGB24 或 YUY2 。 

3) Æ FORMI 中 添加 两 个 “Picture Box ”控件 ，“Picturel ”用 于 以 10ms 的 
固定 间隔 从 视频 流 中 采集 图 像 ， 并 将 其 作为 剪贴 板 数据 Picture2 ”用 于 在 
“Timerl ”控件 下 以 同样 速率 对 所 采集 图 像 进行 处 理 。 

4) 在 “Formn1l1” 中 添加 菜单 项 “Format” ， 用 于 设置 图 像 大 小 为 160 x 120 像 
素 。 

5) 调用 “GetObject”API 从 “Picturel ”控件 获得 图 像 像 素 信 息 。 

6) 在 “Timerl1 ”控件 下 调用 ‘ GetBitmapBits’ API 获得 一 个 8bit 数组 的 像 
素数 组 “Pbytes (c, x, y)”。 “Pbytes” 中 每 个 元 素 均 包含 位 于 图 像 坐 标 “x? 
和 “y” 处 的 8bit RGB 像素 信息 。“c ”代表 颜 色 ; c:2 代表 红色 ; c:1 代表 绿 
色 ; c:0 代表 蓝 色 。 

7) 根据 选项 “Option1” 或 “Option2” 对 像素 数组 进行 处 理 。 

8) 如 果 选 择 “Option1' ， 则 像素 数组 将 通过 程序 “Gray” 处 理 成 灰 度 图 像 ， 
并 调用 “SetBitmapBits” API 在 图 像框 “Picture2” 中 显示 ， 如 图 5.2 所 示 。 

9) 如 果 选 择 'Option2' ， 则 像素 数组 将 首先 按 步 骤 8 所 示人 处 理 成 灰 度 图 像 ， 
然后 调用 程序 “Lowpass” 进 行 低 通 滤波 ， 最 后 在 “ Picture2” 中 显示 ， 如 图 5.2 
所 示 。 
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Option1 Option2 





下 面 的 文本 


Global 
Global 
Global 
Global 
Global 
Global 
Global 
Global 
Global 
Global 
Global 
Global 
Global 


Const 
Const 
Const 
Const 
Const 
Const 
Const 
Const 
Const 
Const 
Const 


Const 


IRIE 


ARE + 低 通 


图 5.2 “Option 1" 和 Option 2° 


给 出 了 “Webcam2. bas” 模 块 的 程序 : 


WS CHILD As Long = &H40000000 
WS VISIBLE As Long = &H10000000 
WM_USER = 1024 





WM CAP 
WM CAP 
WM CAP 
WM CAP. 
WM CAP 
WM CAP 
WM CAP 


DRIVER | CONNECT = WM USER + IO 


_ SET PREVIEW WM USER +50 


SET PREVIEWRATE = WM USER +52 

DRIVER _ DISCONNECT As Long = WM USER +11 
DLG | VIDEOFORMAT As Long = WM USER +41 
GET FRAME As Long 21084 

COPY As Long 2 1054 


WM CAP SET SCALE As Integer = WM USER +53 
SWP NOMOVE As Integer 22 

Const SWP_NOZORDER As Integer 24 

Global Const HWND_ BOTTOM As Integer = | 


Declare Function SendMessage Lib " user32 " Alias " SendMessageA "( ByVal hwnd_ 


As Long, ByVal wMsg As Long, ByVal wParam As Long, ByVal lParam As 


Long) As Long Declare Function capCreateCapture Window Lib 


" avicap32. dll " Alias " 


capCreateCaptureWindowA "( ByVal nWindowName _ 


As String, ByVal nStyle As Long, ByVal nx As Integer, ByVal ny As Integer, _ 
ByVal nWidth As Integer, ByVal nHeight As Integer. ByVal nHwnd As Long, _ 
ByVal nld As Integer) As Long 


Declare Function SetWindowPos Lib " user32 "(ByVal hwnd As Long, - 


ByVal hWndlnsertAfter As Long, ByVal x As Long, ByVal y As Long, 


ByVal ex As Long, ByVal cy As Long. ByVal wFlags As Long) As Long 
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Declare Function GetObiect Lib " gdi32 " Alias " GetObiectA "(ByVal hObject_ 
As Long, ByVal nCount As Long, lpObiect As Any) As Long 
Declare Function GetBitmapBits Lib " gdi32 "(ByVal hBitmap As Long, . 
ByVal dwCount As Long,lpBits As Any) As Long 
Declare Function SetBitmapBits Lib " gdi32 "( ByVal hBitmap As Long, _ 
ByVal dwCount As Long,lpBits As Any) As Long 
图 5.3 28 1H T ‘Form!’ 的 布局 。 


ù Lesson? 








Option | 








Option2 





图 5.3 ‘Form!’ :的 布局 

以 下 文本 给 出 了 “Forml 的 程序 代码 : 
Dim hwde As Long 
Dim startcap As Boolean 
Private Type Bitmap 

bmType As Long 

bmWidth As Long 

bmHeight As Long 

bmWidthBytes As Long 

bmPlanes As Integer 

bmBitsPixel As Integer 

bmBits As Long 
End Type 
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Dim Pbytes( ) As Byte ,Pinfo As Bitmap 
Dim x As Long,y As Long 


Private Sub Form Load( ) 
If App. PrevInstance = True Then End 
Picturel. AutoSize = True 
Picture2. AutoSize = True 
Picturel. ScaleMode = vbPixels 
Picture2. ScaleMode = vbPixels 


Timerl. Interval = 10 


hwdc = capCreateCaptureWindow( " Webcam Vision System ",WS_CHILD_ 
Or WS VISIBLE ,0 ,0 ,160 ,120 Picture 1. hwnd ,0 ) 
If (hwde < >0) Then 
Clipboard. Clear 
If SendMessage( hwde, WM_CAP_DRIVER_CONNECT,0,0) Then 
SendMessage hwdc, WM CAP SET SCALE,1,0 
SendMessage hwde, WM CAP SET PREVIEWRATE,30,0 
SendMessage hwdc, WM CAP SET PREVIEW,1,0 
SetWindowPos hwde, HWND_ BOTTOM,0,0,160,120, 
SWP NOMOVE Or SWP NOZORDER 
SendMessage hwde, WM CAP GET FRAME,0,0 
SendMessage hwde, WM CAP COPY ,0 ,0 
Picturel. Picture = Clipboard. GetData 
GetObject Picturel. Picture , Len( Pinfo) , Pinfo 
ReDim Pbytes(0 To( Pinfo. bmBitsPixel\8 ) —1,0 To Pinfo. bnWidth - 1, — 
0 To Pinfo. bmHeight ~ 1) 
Picture2. height = Picturel. height 
Picture2. width = Picturel. width 
Timerl. Enabled = True 
startcap = True 
Else 
MsgBox " No Webcam found! ",OK. "" 
startcap = False 
Unload Me 
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End If 
Else 
Unload Me 
End If 
End Sub 


Private Sub Gray( width As Long, height As Long) 
Dim G As Byte 
For x =0 To width - 1 
For y =0 To height -1 
G =0. 2125 * CDbl(Pbytes(2,x,y)) +0.7154 * CDbl( Pbytes(1,x, 


yh) + 
0. 0721 * CDbl( Pbytes(0,x,y)) 
Pbytes(2,x,y) =G ' Red 
Pbytes(1,x,y) =G ' Green 
Pbytes(0,x,y) =G ' Blue 
Next y 
Next x 
End Sub 


Private Sub Lowpass( width As Long,height As Long) 
Dim R As Long 
Dim c,d,e,f As Long 
For x 20 To width - 1 
For y =0 To height - 1 


c-zx-1l 


If C «0 Then C = width - 1 

If d = width Then d 20 

If e «0 Then e = height - 1 

If f = height Then f 20 

R = Pbytes(2,x,e) 

R =R + CLng(Pbytes(2,c,y)) 
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R=R+2*CLng(Pbytes(2,x,y)) 
R=R+CLng(Pbytes(2,d,y)) 
R =R + CLng( Pbytes(2,x,f)) 
R=R/6 '3 x3 低 通 
Pbytes(2,x,y) =R 
Pbytes(1,x,y) =R 
Pbytes(0,x,y) =R 
Next y 
Next x 
End Sub 


Private Sub Form_ Terminate( ) 
If startcap = True Then 
SendMessage hwdc, WM_CAP_DRIVER_DISCONNECT,0,0 
startcap = False 
Timerl. Enabled = False 
End If 
End Sub 


Private Sub Form_Unload( Cancel As Integer) 
If startcap = True Then 
SendMessage hwdc, WM CAP DRIVER DISCONNECT,0,0 
startcap = False 
Timerl. Enabled - False 
End If 
End Sub 


Private Sub Menu Click( ) 
If startcap = True Then 
SendMessage hwdc, WM CAP DLG  VIDEOFORMAT,0,0 
End If 
End Sub 


Private Sub Timerl Timer( ) 
Timerl. Enabled - False 
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SendMessage hwdc, WM CAP GET FRAME,0,0 

SendMessage hwdc, WM CAP COPY ,0 ,0 

Picture 1. Picture = Clipboard. GetData 

GetBitmapBits Picturel. Picture. Pinfo. bnWidthBytes * Pinfo. bmHeight, _ 
Pbytes(0,0,0) 

If Option 1. Value = True Then Gray Picturel. ScaleWidth. Picturel. Scale- 


Height 
If Option2. Value = True Then 
Gray Picturel. ScaleWidth. Picturel. ScaleHeight 
Lowpass Picturel. ScaleWidth. Picturel. ScaleHeight 
End If 
SetBitmapBits Picture2. Image , Pinfo. bmWidthBytes * Pinfo. bmHeight, _ 
Pbytes(0,0,0) 
Picture2. Refresh 
Picture2. Picture = Picture2. Image 
Timerl. Enabled = True 
End Sub 
低 通 滤波 是 通过 如 下 所 示 的 一 个 3 x3 AY 2D FIR 滤波 需 来 实现 的 : 
i 0 1 0 
* 1 2 1 
0 1 0 


对 上 述 掩 膜 进 行 2D 循环 卷 积 来 保存 进行 最 小 失真 滤波 前 后 的 图 像 大 小 。 

如 果 采 集 图 像 的 大 小 与 图 像框 不 符 ， 则 需 激活 formal 策 单 将 图 像 大 小 转换 
为 160 x120 像素 。 如 果 没 有 可 用 的 网 络 摄像 头 ， 则 会 在 消息 框 中 弹出 消息 “未 
找到 任何 网 络 摄像 头 ! (No webcam found! )” . 


5.4 示例 3 


目标 : 开发 一 个 VB6 程序 用 于 采集 和 处 理 网 络 视频 流 ， 并 进行 灰 度 图 像 转 
换 、 低 通 图 像 滤 波 和 对 比 度 增强 处 理 。 

以 下 步骤 对 程序 开发 过 程 进行 概述 : 

1) 与 示例 2 中 所 述 的 “Webcam2. bas” 相 同 ， 所 有 必需 的 API 调用 均 在 
‘Webcam3. bas? 模块 中 声明 。 同 时 在 VB6 程序 的 “Forml ”中 必须 包含 该 模块 

2) AVICAP32. DLL 用 于 通过 调用 合适 的 API 来 采集 网 络 视 频 流 。 网 络 摄像 
头 视频 格式 应 为 RGB24 或 YUY2 。 
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3) Æ ‘Forml’ 中 添加 两 个 “Picture Box” 控 件 ，“Picturel ”用 于 以 10ms 
的 固定 间隔 从 视频 流 中 采集 图 像 ， 并 将 其 作为 剪贴 板 数据 ,Picture2 "在 
* Timerl "控件 下 以 同样 的 速率 对 所 采集 的 图 像 进行 处 理 

4) 在 “Forml1” 中 添加 菜单 项 “Format”， 用 于 设置 图 像 大 小 为 160 x120 f& 

5) HiH ‘GetObject? API, M “Picturel ”中 获得 图 像 像 素数 据 信息 

6) 在 “Timerl” 控件 下 通过 调用 “GetBitmapBits ” API 获得 一 个 8bit 的 T 
素数 组 “Pbytes (c, x, y)’. “Pbytes” 中 的 每 个 元 素 均 包含 图 像 坐 标 “x” 和 
“y” 处 每 个 像素 的 8bit RGB RAH. ‘ce’ (ABIL; c:2 代表 红色 ; c:1 fX 
表 绿 色 ; c:0 代表 蓝 色 。 

7) 根据 选项 控制 “Option1”、“ Option2” 或 “Option3” 来 处 理 像 素数 组 

8) 如 果 选 择 “Option1”， 则 在 程序 “Gray” 下 像素 数组 被 处 理 成 灰 度 图 像 ， 
并 调用 'SetBitmapBits' API 在 图 像框 “Picture2” 中 显示 ， 如 图 5.4 所 示 。 


Option | Option2 Option3 





AE AKHE + 低 通 灰 度 + 低 通 + 对 比 度 
图 5.4 ‘Option!’ , I 和 ‘ Option3’ 

9) 如 果 选 择 ‘Option2’ , WIFE 8 所 述 ， 首 先 将 像素 数组 处 理 成 灰 度 图 像 ， 
然后 在 程序 “Lowpass” 下 进行 低 通 滤波 ， 最 后 显示 在 “Picture2 ”中 ， 如 图 5.4 
所 示 

10) 如 果 选 择 “Option3”， 则 按 步 又 9 所 述 对 数组 进行 低 通 滤波 ， 青 在 程序 
“Contrast ”下 应 用 直方 图 拉 伸 技术 进行 对 比 度 增强 ， 最 后 显示 在 “Picture2 ”中 ， 
如 图 5.4 所 示 。 

图 5.5 给 出 了 “Formnl” 的 布局 

下 面 的 文本 显示 了 “Contrast ”和 “Timerl ”的 程序 代码 ， 其 余 代 但 请 参见 
示例 2 

Private Sub Contrast( width As Long, height As Long) 

Dim R As Long ' 直 方 图 拉 伸 
Dim pmax,pmin As Long 
pmax =0 


pmin = 255 
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图 5.5 “Forml” 的 布局 
For x =0 To width - 1 
For y 20 To height - 1 
If pmax < = CLng( Pbytes(2,x,y) ) Then pmax = Pbytes(2,x,y) 
If pmin > = CLng( Pbytes(2,x,y) ) Then pmin = Pbytes(2,x,y) 
Next y 
Next x 
For x =0 To width - 1 
For y 20 To height - 1 
R = Pbytes(2,x, v) 
If pmax > pmin Then R = (((R - pmin) * 255)/( pmax - pmin) ) + 


pmin/4 
If R «0 Then R 20 
If R » 255 Then R 2255 
Pbytes(2,x,y) =R 
Pbytes(1,x,y) =R 
Pbytes(0,x,y) =R 
Next y 
Next x 
End Sub 


Private Sub Timerl Timer( ) 


AE 


8 5d 基于 视觉 的 移动 机 器 人 导航 算法 的 实现 示例 107 





Timerl. Enabled = False 
SendMessage hwdc, WM_CAP_GET_FRAME,0,0 
SendMessage hwde, WM CAP COPY ,0,0 
Picture 1. Picture = Clipboard. GetData 
GetBitmapBits Picturel. Picture , Pinfo. bmWidthBytes * Pinfo. bmHeight, _ 
Pbytes(0,0,0) 
If Optionl. Value = True Then Gray Picturel. ScaleWidth. Picturel. ScaleHeight 
If Option2. Value = True Then 
Gray Picturel. ScaleWidth. Picturel. ScaleHeight 
Lowpass Picturel. ScaleWidth. Picturel. ScaleHeight 
End If 
If Option3. Value = True Then 
Gray Picturel. ScaleWidth. Picturel. ScaleHeight 
Lowpass Picturel. ScaleWidth. Picturel. ScaleHeight 
Contrast Picturel. ScaleWidth. Picturel. ScaleHeight 
End If 
SetBitmapBits Picture2. Image. Pinto. bmWidthBytes * Pinfo. bmHeight, _ 
Pbytes(0,0,0) 
Picture2. Reflesh 
Picture2. Picture = Picture2. Image 
Timerl. Enabled = True 
End Sub 
如 果 采 集 图 像 的 大 小 与 图 像框 不 符 ， 那 么 将 激活 “Format” 菜 单 使 得 图 像 大 
小 转换 为 160 x120 像素 。 如 果 没 有 可 用 的 网 络 摄像 头 ， 那 么 会 在 消息 框 中 弹出 
一 条 消息 “未 找到 任何 网 络 摄像 头 ! (No webcam found!) " 


5.5 示例 4 


目标 : 开发 一 个 VB6 程序 用 于 采集 和 处 理 网 络 视频 流 ， 进 行 灰 度 图 像 转 换 、 
低 通 图 像 滤 波 、 对 比 度 增 强 和 几何 均值 滤波 处 理 。 

以 下 步骤 对 程序 开发 过 程 进行 概述 : 

1) 与 示例 3 中 所 述 的 “Webcam3. bas” 相 同 ， 所 有 必需 的 API 调用 均 在 
“Webcam4. bas ”模块 中 声明 。 同 时 在 VB6 程序 的 “Forml， 中 必须 包含 该 模块 。 

2) AVICAP32. DLL 用 于 通过 调用 合适 的 APT 来 采集 网 络 视频 流 。 网 络 摄像 
头 视频 格式 应 为 RGB24 或 YUY2。 
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3) Æ FORMI 中 添加 两 个 “Picture Box ”控件 ，“Picturel ”用 于 以 10ms 的 
固定 间隔 从 视频 流 中 采集 图 像 ， 并 将 其 作为 剪贴 板 数 据 , “Picture2' 在 “Timerl ' 
控件 下 以 同样 的 速率 对 所 采集 的 图 像 进 行 处 理 。 

4) 在 “Forml ”中 添加 菜单 项 “Format”， 用 于 设置 图 像 大 小 为 160 x 120 像 
素 。 

5) 通过 调用 “GetObject”API， 从 “Picturel” 中 获得 图 像 像 素数 据 信 息 。 

6) 在 “Timerl ”控件 下 通过 调用 “GetBitmapBits ” API 获得 一 个 8bit 的 像 
素数 组 “Pbytes (ec，x，y)”。“Pbytes” 中 的 每 个 元 素 均 包含 图 像 坐标 “x” 和 
“y” 处 每 个 像素 的 8bit RGB 像素 信息 。“c” 代 表 颜 色 ; c:2 代表 红色 ; c:1 fX 
表 绿 色 ; c:0 AKEE. 

7) He He VEL FE ii] 'Optionl', 'Option2' 、“ Option3” 或 “Option4” 来 处 理 
像素 数组 。 

8) 如 果 选 择 “Option1”， 则 在 程序 “Gray” 下 像素 数组 被 处 理 成 灰 度 图 像 ， 
并 调用 “SetBitmapBits”API 在 图 像框 “Picture2” 中 显示 ， 如 图 5.6 所 示 。 

9) 如 果 选 择 'Option2' ， 则 按 步骤 8 所 述 ， 首 先 将 像素 数组 处 理 成 灰 度 图 
像 ， 然 后 在 程序 “Lowpass” 下 进行 低 通 滤 波 ， 最 后 显示 在 “Picture2 ”中 ， 如 图 
5.6 所 示 。 

10) 如 果 选 择 “ Option3” ， 则 按 步 最 9 所 述 对 数组 进行 低 通 滤波 ， 再 在 程序 
“Contrast” 下 应 用 直方 图 拉 伸 技术 进行 对 比 度 增强 ， 最 后 显示 在 “ Picture2’ tH, 
如 图 5.6 所 示 。 

11) 如 果 选 择 ‘Options’ ， 则 按 步 又 10 所 述 对 数组 进行 对 比 度 增强 ， 再 在 
程序 “Geometricmean ”下 进行 几何 均值 滤波 ， 最 后 显示 在 “Picture2 ”中 ， 如 图 
5.6 所 示 。 提 供 增加 几何 均值 滤波 器 个 数 以 及 每 个 滤波 器 窗口 大 小 的 选择 。 

图 5.7 E T ‘Fom!’ 的 布局 。 

下 面 的 文本 显示 了 “Geometricmean ”和 “Timerl ”的 程序 代码 ， 其 余 代 码 

Private Sub Geometricmean( width As Long, height As Long,Size As Long) 

Dim R,S As Long 

Dim i,j As Long 

Dim c,d As Long 

Dim wl,hl As Long 

If Size <3 Then Size =3 

If Size > 7 Then Size =7 

If (Size And 1) =0 Then Size = Size + 1 奇偶 转换 


S = Size * Size 
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Option2 Option3 





灰 度 IKIE + 低 通 ARE + 低 通 + 对 比 度 


Option4 





灰 度 + 低 通 + 对 比 度 + 几何 均值 
图 5.6 “Option1”、“Option2 ”、“ Option3 ”和 'Option4" 


™ Lesson4 





图 5.7 “Forml ”的 布局 


wl =width -1 
hl = height - 1 
For x =0 To wl 
For y =0 To hl 
Rel 
For 1=0 To Size - 1 


For j =0 To Size - 1 
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c=x+t+i-((Size-1)/2) 
If c <0 Then c = width + C 
If c >wl Then czc- wl 
d =y +j- ( (Size -1)/2) 
If d <0 Then d = height + d 
If d» hl Then d =d -hl 
R =R * CLng( Pbytes(2,c,d) ) 
Next j 

Next i 

R = R*(1#/S) 

If R » 255 Then R =255 

Pbytes(2,x,y) =R 

Pbytes(1,x,y) =R 

Pbytes(0,x,y) =R 

Next y 
Next x 


End Sub 


Private Sub Timerl Timer( ) 
Timerl. Enabled - False 
SendMessage hwdc, WM CAP GET FRAME,0,0 
SendMessage hwdc, WM CAP COPY,,0,0 
Picturel. Picture = Clipboard. GetData 
GetBitmapBits Picturel. Picture. Pinfo. bmWidthBytes * Pinfo. bmHeight, _ 
Pbytes(0,0,0) 
If Optionl. Value = True Then Gray Picturel. ScaleWidth , Picturel. ScaleHeight 
If Option2. Value = True Then 
Gray Picturel. ScaleWidth. Picturel. ScaleHeight 
Lowpass Picturel. ScaleWidth. Picturel. ScaleHeight 
End If 
If Option3. Value = True Then 
Gray Picturel. ScaleWidth. Picturel. ScaleHeight 
Lowpass Picturel. ScaleWidth. Picturel. ScaleHeight 
Contrast Picturel. ScaleWidth. Picturel. ScaleHeight 
End If 
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If Option4. Value = True Then 
Gray Picturel. ScaleWidth. Picturel. ScaleHeight 
Lowpass Picturel. ScaleWidth. Picturel. ScaleHeight 
Contrast Picturel. ScaleWidth. Picturel. ScaleHeight 
For i =1 To Val( Text4. Text) 
Geometricmean Picturel. ScaleWidth. Picturel. ScaleHeight. gms 
Next i 
End If 
SetBitmapBits Picture2. Image , Pinfo. bmWidthBytes * Pinfo. bmHeight, - 
Pbytes(0,0,0) 
Picture2. Refresh 
Picture2. Picture = Picture2. Image 
Timerl. Enabled = True 
End Sub 
如 果 采 集 图 像 的 大 小 与 图 像框 不 符 ， 那么 将 激活 “Format” 菜 单 使 得 图 像 大 
小 转换 为 160 x 120 像素 。 如 果 没 有 可 用 的 网 络 摄像 尖 ， 那么 会 在 消息 框 中 弹出 
一 条 “未 找到 任何 网 络 摄像 头 ! (No webcam found!)” 的 消息 。 


5.6 示例 5 


目标 : 开发 一 个 VB6 程序 用 于 采集 和 处 理 网 络 视频 流 ， 进 行 灰 度 图 像 转 换 、 
低 通 图 像 滤 波 、 对 比 度 增 强 、 几 何 均值 滤波 和 自 适 应 阔 值 操作 ， 在 光照 变化 条 
件 下 ， 从 采集 图 像 中 提取 白色 路 径 。 

以 下 步骤 对 程序 开发 过 程 进行 概述 : 

1) 与 示例 4 中 所 述 的 “Webcam4.bas” 相 同 ， 所 有 必需 的 API 调用 均 在 
“Webcam5. bas” 模 块 中 声明 。 同 时 在 VB6 程序 的 “Forml” 中 必须 包含 该 模块 。 

2) AVICAP32. DLL 用 于 通过 调用 合适 的 API 来 采集 网 络 视频 流 。 网 络 摄像 
头 视频 格式 应 为 RGB24 或 YUY2 

3) Æ FORMI 中 添加 两 个 “Picture Box” 控 件 ，“Picturel ”用 于 以 10ms 的 
固定 间隔 从 视频 流 中 采集 图 像 ， 并 将 其 作为 剪贴 板 数 据 ，Picture2 ’ fE * Timerl ’ 
控件 下 以 同样 的 速率 对 所 采集 的 图 像 进行 处 理 。 

4) 在 “Forml1” 中 添加 菜单 项 “Format”， 用 于 设置 图 像 大 小 为 160 x 120 像 
素 。 

5) 通过 调用 “CetObject”APLI， 从 “Picturel ”中 获得 图 像 像 素数 据 信息 。 

6) 在 “Timerl ”控件 下 通过 调用 “GetBitmapBits ” API 获得 一 个 8bit 的 像 
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素数 组 “Pbytes (c, x, y) ”。“Pbytes” 中 的 每 个 元 素 均 包含 图 像 坐 标 “x” 和 
“y” 处 每 个 像素 的 8bit RGB 像素 信息 。“c” 代 表 颜 色 ; c:2 代表 红色 ; c:1 代 
表 绿 色 ; c:0 代表 蓝 色 ， 
7) 根据 选项 控制 “Option1” , 'Option2' , 'Option3' , "Optiond" 或 “Op- 
tion5” 来 处 理 像 素数 组 
8) 如 果 选 择 “Option1”， 则 在 程序 “Gray” 下 像素 数组 被 处 理 成 灰 度 图 像 ， 
并 调用 “SetBitmapBits ”API 在 图 像框 “Picture2 ”中 显示 
WREE ‘Option?’ , WEARS 所 述 ， 首 先 将 像素 数组 处 理 成 灰 度 图 
像 ， 然 后 在 程序 “Lowpass” 下 进行 低 通 滤波 ， 最 后 显示 在 “Picture2 ”中 
10) 如 果 选 择 “Option3” ， 则 按 步 又 9 所 述 对 数组 进行 低 通 滤波 ， 再 在 程序 
“Contrast ”下 应 用 直方 图 拉 伸 技术 进行 对 比 度 增 强 ， 最 后 显示 在 “Picture2 ”中 
) 如 果 选 择 “Option4 , Wi Fe 2b HE 10 所 述 对 数组 进行 对 比 度 增强 ， 再 在 
程序 “Geometricmean ”下 进行 几何 均值 滤波 ， 最 后 显示 在 “Picture2” 中 。 提 供 
增加 几何 均值 滤波 器 个 数 以 及 每 个 滤波 器 窗口 大 小 的 选择 
12) 如 果 选 择 'OptionS' , ， 则 调用 程序 
‘ Adaptive Threshold’ ”进行 自 适应 靖 值 操作 ， 
并 在 “Picture2 ”中 显示 ， 如 图 5.8 所 示 。 首 
先 根 据 程 序 “WhiteLineWidth” 确定 参考 像素 
[在 标 称 位 置 (80，110)] 附近 的 白色 路 
径 宽度 。 如 果 参 考 像素 周围 的 左 路 径 宽 度 和 右 
宽度 小 于 “MIN_PATH_WIDTH” 值 ， 则 
调节 参数 “delta” ， 通 过 在 “delta_max” 范 围 
内 减 小 阔 值 来 增 大 路 径 宽度 。 然 后 通过 程序 SENE ee 
“Threshold ”来 计算 新 图 像 ， 并 重复 上 述 操 作 ， 直 到 获得 一 个 有 效 的 白色 路 径 
图 5.9 给 出 了 “Forml1” 的 布局 。 
接 下 来 的 文本 显示 了 ‘ AdaptiveThreshold’ 、“ WhiteLineWidth”、“ Threshold" 
和 “Timerl ”的 程序 代码 ， 其 余 代 码 请 参见 示例 4 
Private Sub AdaptiveThreshol d( width As "PARE As Long,yr As Long) 


Option5 





ARNE + (od + 对 比 度 + 几何 均值 + BAL 


Dim 1 As Integer 

WhiteLineWidth width, xr, yr 

If PixelCountLeft < MIN PATH. WIDTH And PixelCountRight < _ 
MIN PATH WIDTH Then 
delta = delta +0. 2 
If delta > delta max Then 


delta = delta max 
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Format 





K| 5.9 'Forml' 的 布局 
Else 
GoTo ate 
End If 
If delta < 1#Then delta = 1# 
End If 
delta = delta ~ 0. 5 
atc: 
i = Pbytes(2 , xr, yr) 
If i > (255 - (2 * delta) ) Then 
If i > (255 - delta) Then i = (255 - delta) 
Threshold Picturel. ScaleWidth , Picturel. ScaleHeight,i — CInt( delta) , — 
i + CInt( delta) 
Else 
Threshold Picturel. ScaleWidth , Picturel. ScaleHeight ,255 ,255 
End If 
End Sub 


Private Sub WhiteLineWidth( width As Long,xr As Long,yr As Long) 
Dim pcll , pcl2, pcl3, perl ,pcr2 , per3 As Integer 
PixelCountLeft = 0 ; PixelCountRight =0 
y=yr 


pell =0:pcrl 20 
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For x = xr To 0 Step - 1 
If Pbytes(2,x,y) >250 Then 
pcll = pcll +1 
End If 
Next x 
For x = (xr +1) To( width - 1) 
If Pbytes(2,x,y) >250 Then 
perl = perl +1 
End If 
Next x 
y=yr-1 
pcl2 =0: per2 =0 
For x = xr To 0 Step - 1 
If Pbytes(2,x,y) >250 Then 
pel2 = pcl2 +1 
End If 
Next x 
For x = (xr +1) To( width - 1) 
If Pbytes(2,x,y) »250 Then 
per2 = per2 +1 
End If 
Next x 
y=yrt+l 
pel3 =0: per3 =0 
For x = xr To 0 Step - 1 
If Pbytes(2,x,y) »250 Then 
pcl3 = pcl3 +1 
End If 
Next x 
For x = (xr +1) To( width - 1) 
If Pbytes(2,x,y) »250 Then 
Per3 = per3 + 1 
End If 


Next x 
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PixelCountLeft = ( pcll + pcl2 + pcl3 ) /3 
PixelCountRight = ( perl + pcr2 + per3 ) /3 
End Sub 
Private Sub Threshold( width As Long, height As Long,lv As Long,hv As Long) 
Dim R As Long 
For x =0 To width -1 
For y =0 To height -1 
R = Pbytes(2,x,y) 
If R «1v Then R 20 
If R > z hv Then R 2255 
Pbytes(2,x,y) =R 
Pbytes(1,x,y) =R 
Pbytes(0,x,y) =R 
Next y 
Next x 
End Sub 


Private Sub Timer] _ Timer( ) 
Timerl. Enabled = False 
SendMessage hwdc, WM CAP GET FRAME,0,0 
SendMessage hwdc, WM CAP COPY,0,0 
Picturel. Picture = Clipboard. GetData 
GetBitmapBits Picturel. Picture , Pinfo. bmWidthBytes * Pinfo. bmHeight, _ 
Pbytes(0,0,0) 
If Optionl. Value = True Then Gray Picturel. ScaleWidth. Picturel. Scale- 
Height 
If Option2. Value = True Then 
Gray Picturel. ScaleWidth. Picturel. ScaleHeight 
Lowpass Picturel. ScaleWidth. Picturel. ScaleHeight 
End If 
If Option3. Value = True Then 
Gray Picturel. ScaleWidth, Picturel. ScaleHeight 
Lowpass Picturel. ScaleWidth. Picturel. ScaleHeight 
Contrast Picturel. ScaleWidth , Picturel. ScaleHeight 
End If 
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If Option4. Value = True Then 
Gray Picturel. ScaleWidth, Picturel. ScaleHeight 
Lowpass Picturel. ScaleWidth. Picturel. ScaleHeight 
Contrast Picturel. ScaleWidth. Picturel. ScaleHeight 
For i = 1 To Val( Text4. Text) 
Geometricmean Picturel. ScaleWidth , Picturel. ScaleHeight , gms 
Next i 
End If 
If OptionS. Value = True Then 
Gray Picture 1. ScaleWidth, Picturel. ScaleHeight 
Lowpass Picturel. ScaleWidth , Picturel. ScaleHeight 
Contrast Picturel. ScaleWidth , Picturel. ScaleHeight 
For i=1 To Val( Text4. Text ) 
Geometricmean Picture]. ScaleWidth, Picturel. ScaleHeight , gms 
Next i 
AdaptiveThreshold Picturel. ScaleWidth, Val ( Text2. Text ), Val 
( Texi3. Text) 
End If 
SetBitmapBits Picture2. Image. Pintb. bmWidthBytes * Pinfo. bmHeight, _ 
Pbytes(0,0,0) 
Picture2. Refresh 
Picture2. Picture = Picture2. Image 
Picture2. Line ( Val ( Text2. Text) - 2, Val ( Tex3. Text) - 2) - ( Val 
( Text2. Text) +2,_ 
Val( Text3. Text) +2) ,RGB(255,0,0) ,B 
Timerl. Enabled = True 
End Sub 
如 果 采 集 图 像 的 大 小 与 图 像框 不 符 ， 那 么 将 激活 “Format” 菜 单 使 得 图 像 大 
小 转换 为 160 x120 像素 。 如 果 没 有 可 用 的 网 络 摄像 头 ， 那 么 会 在 消息 框 中 弹出 
一 条 “未 找到 任何 网 络 摄像 头 ! (No webcam found!)” 的 消息 。 


5.7 示例 6 


目标 : 开发 一 个 VB6 程序 用 于 采集 和 处 理 网 络 视 频 流 ， 进 行 灰 度 图 像 转 换 、 
低 通 图 像 滤 波 、 对 比 度 增 强 、 几 何 均值 滤波 、 自 适应 效 值 以 及 清除 操作 ， 在 光 
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照 变化 条 件 下 ， 从 采集 图 像 中 提取 白色 路 径 并 清除 不 必要 物体 。 

以 下 步骤 对 程序 开发 过 程 进行 概述 

1) 与 示例 5 中 所 述 的 “Webcam5. bas” 相 同 ， 所 有 必需 的 APL 调用 均 在 
'Webcam6. bas” 模 块 中 声明 。 同 时 在 VB6 程序 的 “Forml” 中 必须 包含 该 模块 。 

2) AVICAP32. DLL 用 于 通过 调用 合适 的 API 来 采集 网 络 视频 流 。 网 络 摄像 
头 视 频 格 式 应 为 RGB24 或 YUY2。 

3) 在 FORMI1 中 添加 两 个 “Picture Box’ 控件，“Picturel1 ”用 于 以 10ms 的 
固定 间隔 从 视频 流 中 采集 图 像 ， 并 将 其 作为 剪贴 板 数据 ，Picture2 " f£ * Timer! ' 
控件 下 以 同样 的 速率 对 所 采集 的 图 像 进行 处 理 。 

4) 在 “Forml” 中 添加 菜单 项 “Format”， 用 于 设置 图 像 大 小 为 160 x120 像 

5) 通过 调用 “GetObject”API， 从 “Picturel ”中 获得 图 像 像素 数据 信息 。 

6) 在 “Timerl1” 控 件 下 通过 调用 “GetBitmapBits”APIl 获得 一 个 8bit 的 像 
素数 组 “Pbytes (c, x, y)'。“Pbytes ”中 的 每 个 元 素 均 包含 图 像 坐标 “x” 和 
“y” 处 每 个 像素 的 8bit RGB 像素 信息 。“c” 代 表 颜 色 ; c:2 代表 红色 ; c:1 fX 
表 绿 色 ; c:0 代表 蓝 色 。 

7) 根据 选项 控制 “Option1”、“ Option2' , ‘ Option3 ' , " Option4”、“ Options” BK 
“Option6 "来 处 理 像素 数组 。 

8) 如 果 选 择 “Option1” ， 则 在 程序 “Gray” 下 像素 数组 被 处 理 成 灰 度 图 像 ， 
并 调用 SetBitmapBits” API 在 图 像框 “Picture2 ”中 显示 。 

9) 如 果 选 'Option2' ， 则 按 步 又 8 所 述 ， 首先 将 像素 数组 处 理 成 灰 度 图 
像 ， 然 后 在 程序 “Lowpass” 下 进行 低 通 滤 波 ， 最 后 显示 在 “Picture2” 中 。 

10) 如 果 选 择 “Option3 , MEZHER 9 所 述 对 数组 进行 低 通 滤 波 ， 再 在 程序 
“Contrast ”下 应 用 直方 图 拉 伸 技 术 进 行 对 比 度 增强 ， 最 后 显示 在 “Picture2 ”中 。 

11) 如 果 选 择 “Option4”， 则 按 步 又 10 所 述 对 数组 进行 对 比 度 增强 ， 再 在 
程序 “Geometricmean” 下 进行 几何 均值 滤波 ,最 后 显示 在 “Picture2” 中 。 提 供 
增加 几何 均值 滤波 器 个 数 以 及 每 个 滤波 闫 窗口 大 小 的 选择 。 

12) 如 果 选 择 'OptionS' ， 则 调用 程序 “ Adaptive Threshold ”进行 自 适 应 闭 
值 操作 ， 并 在 “Pieture2 ”中 显示 。 首 先 根据 程序 “ 允 hiteLineWidth” 确定 参考 
像素 点 [在 标 称 位 置 (80, 110)] 附近 的 白色 路 径 宽度 。 如 果 参 考 像 素 周 围 的 
左 路 径 宽度 和 右 路 径 宽度 小 于 “MIN_PATH_WIDTH” 值 ， 则 调节 参数 “del- 
ta’, IE ‘delta max’ 范围 内 减 小 国 值 来 增 大 路 径 宽 度 。 然 后 通过 程序 
“Threshold ”来 计算 新 图 像 ， 并 重复 上 述 操作 ， 直 到 获得 一 个 有 效 的 白色 路 径 。 

13) 如 果 选 择 “Option6 ， 则 调用 程序 ‘Clean’ ,执行 一 个 附加 的 清除 操作 
来 去 除 不 必要 物体 ， 并 在 “Picture2” 中 显示 ， 如 图 5.10 所 示 。 
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图 5.11 给 出 了 “Forml” 的 布局 Option6 


接 下 x p 的 X AS l TE 7 3d * Clean à n 
“Timerl ”的 程序 代码 ， 其 余 代 码 请 参见 
示例 5: 


Private Sub Clean ( width As Long, 





height As Long,yr As Long) 
Dim R. xr,xref,xwidth As Long 
Dim PB As Long 
Dim bl_ flag As Boolean Kl 5. 10 Option6 ’ 


ARSE + 低 通 + 对 比 度 + 几何 均值 + REL + 清除 


= Lesson6 


Format 


Captured 


Gray (* 
Gray+lowpass C 
` Gray+lowpass+contrast C 


Gray+lowpass+contrast+geometricmean 全 Reference pixel (2.2)-(157,117)} 


. Gray+lowpass +contrast+geometricmean+threshold C : X| 80 Y|110 à 
Ó , Brayslowpassecantrastsgeometriemeanthreshold«clean ~ Default | ao 





Fd 5.11 “Forml” 的 布局 

bl_ flag = False 
xref =0 
xwidth =0 
If PixelCountLeft > =MIN_ PATH WIDTH Or PixelCountRight > = 
MIN PATH WIDTH Then 
For x 20 To width - 1 

R = Pbytes(2,x,yr) 

If R > 240 Then 

If xref =0 Then xref = x 

End If 

If R 2240 And xref » 0 Then xwidth = xwidth + 1 
Next x 


xr = xref + ( xwidth/2 ) 
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For y = height - 1 To( yr - 1) Step - 1 
For x 0 To width - 1 
Pbytes(2,x,y) =0 
Pbytes(1,x,y) 20 
Pbytes(0,x,y) =0 
Next x 
Next y 
For y = yr To 0 Step - 1 

For x 2 xr To 0 Step - 1 
R = Pbytes(2,x,y) 

If bl_ flag = True Then GoTo ml 

If R «240 Then 
PB =x 
If PB = xr Then bl flag = True 
GoTo ml 

End If 

Next x 
ml: 

For x = PB To 0 Step - 1 
Pbytes(2,x,y) =0 
Pbytes(1,x,y) =0 
Pbytes(0,x,y) =0 

Next x 


For x = (xr +1) To width - 1 
R = Pbytes(2,x,y) 
If bl_ flag = True Then GoTo m2 
If R «240 Then 
PB =x 
If PB = (xr+1) Then bl_flag = True 
GoTo m2 
End If 
Next x 
m2; 
For x = PB To width -1 
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Pbytes(2,x,y) =0 

Pbytes(1,x,y) =0 

Pbytes(0,x,y) =0 
Next x 


xref =0 
xwidth =0 
For x =0 To width -1 
R = Pbytes(2,x,y) 
If R >240 Then 
If xref =O Then xref =x 
End If 
If R >240 And xref >0 Then xwidth = xwidth + 1 
Next x 
If xwidth 20 Then bl flag = True 
For x 20 To width -1 
If bl flag = True Then 
Pbytes(2,x,y) =0 
Pbytes(1,x,y) =0 
Pbytes(0,x,y) 20 
End If 
Next x 


Next y 
End If 
End Sub 


Private Sub Timerl Timer( ) 


Height 


Timerl. Enabled - False 

SendMessage hwdc, WM CAP GET FRAME,0,0 

SendMessage hwdc, WM CAP COPY ,0,0 

Picturel. Picture = Clipboard. GetData 

GetBitmapBits Picturel. Picture. Pinfo. bmWidthBytes * Pinfo. bmHeight, _ 
Pbytes(0,0,0) 

If Optionl. Value = True Then Gray Picturel. ScaleWidth, Picturel. Scale- 
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If Option2. Value = True Then 
Gray Picturel. ScaleWidth. Picturel. ScaleHeight 
Lowpass Picturel. ScaleWidth. Picturel. ScaleHeight 
End If 
If Option3. Value = True Then 
Gray Picturel. ScaleWidth. Picturel. ScaleHeight 
Lowpass Picturel. ScaleWidth. Picturel. ScaleHeight 
Contrast Picturel. ScaleWidth. Picturel. ScaleHeight 
End If 
If Option4. Value = True Then 
Gray Picturel. ScaleWidth. Picturel. ScaleHeight 
Lowpass Picturel. ScaleWidth. Picturel. ScaleHeight 
Contrast Picturel. ScaleWidth. Picturel. ScaleHeight 
For i=1 To Val( Text4. Text) 
Geometricmean Picturel. ScaleWidth. Picturel. ScaleHeight , gms 
Next i 
End If 
If OptionS. Value = True Then 
Gray Picturel. ScaleWidth, Picturel. ScaleHeight 
Lowpass Picturel. ScaleWidth. Picturel. ScaleHeight 
Contrast Picturel. ScaleWidth. Picturel. ScaleHeight 
For i=1 To Val( Text4. Text) 
Geometricmean Picturel. SealeWidth, Picturel. ScaleHeight , gms 
Next i 
AdaptiveThreshold Picturel. ScaleWidth, Val ( Text2. Text ) , Val ( Text3. 
Text) 
End If 
If Option6. Value = True Then 
Gray Picturel. ScaleWidth. Picturel. ScaleHeight 
Lowpass Picturel. ScaleWidth. Picturel. ScaleHeight 
Contrast Picturel. ScaleWidth. Picturel. ScaleHeight 
For i=1 To Val( Text4. Text) 
Geometricmean Picturel. ScaleWidth , Picturel. ScaleHeight , gms 
Next 1 
AdaptiveThreshold Picturel. ScaleWidth , Val( Text2. Text) , 
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Val(Text3. Text) 
Clean Picturel. ScaleWidth, Picturel. ScaleHeight, Val( Text3. Text) 
End If 
SetBitmapBits Picture2. Image , Pinto. bmWidthBytes * Pinfo. bmHeight, _ 
Pbytes(0,0,0) 
Picture2. Refresh 
Picture2. Picture = Picture2. Image 
Picture2. Line ( Val ( Texi2. Text) - 2, Val ( Text3. Text) ~ 2) - ( Val 
( Text2. Text) _ 
+2, Val( Texi3. Text) +2) ,RGB(255,0,0) ,B 
Timerl. Enabled = True 
End Sub 
如 果 采 集 图 像 的 大 小 与 图 像框 不 符 ， 那 么 将 激活 “Format” 菜 单 使 得 图 像 大 
小 转换 为 160 x120 像素 。 如 果 没 有 可 用 的 网 络 摄像 头 ， 那 么 会 在 消息 框 中 弹出 
一 条 “未 找到 任何 网 络 摄像 头 ! (No webcam found!1)” 的 消息 。 


5.8 示例 7 


目标 : 开发 一 个 VB6 程序 用 于 采集 和 处 理 网 络 视频 流 ， 进 行 灰 度 图 像 转换 、 
低 通 图 像 滤 波 、 对 比 度 增强 、 几 何 均值 滤波 、 自 适应 效 值 、 清 除 操作 ， 并 在 基 
于 视觉 的 导航 中 选择 黑色 /白色 路 径 ， 

以 下 步骤 对 程序 开发 过 程 进行 概述 : 

1) 与 示例 6 中 所 述 的 “Webcam6.bas” 相同 ， 所 有 必需 的 API 调用 均 在 
“Webcam7. bas ”模块 中 声明 。 同 时 在 VB6 程序 的 “Formnl” 中 必须 包含 该 模块 。 

2) AVICAP32. DLL 用 于 通过 调用 合适 的 API 来 采集 网 络 视频 流 。 网 络 摄像 
头 视频 格式 应 为 RGB24 或 YUY2 。 

3) Æ FORMI 中 添加 两 个 “Picture Box’ 控件 ，“Picturel1 ”用 于 以 10ms 的 
固定 间隔 从 视频 流 中 采集 图 像 ， 并 将 其 作为 剪贴 板 数据 Picture2 " 在 “Timerl” 
控件 下 以 同样 的 速率 对 所 采集 的 图 像 进行 处 理 。 

4) 在 “Forml” 中 添加 菜单 项 “Format”， 用 于 设置 图 像 大 小 为 160 x120 像 
素 。 

5) 通过 调用 “GetObject”API， 从 “Picturel ”中 获得 图 像 像 素数 据 信 息 。 

6) 在 “Timerl” 控件 下 通过 调用 “GetBitmapBits ”API 获得 一 个 8bit 的 像 
素数 组 “Pbytes (c, x, y)'. 'Pbytes' 'HHJA&ET 262 HIBLA KE AS x" 和 
“y” 处 每 个 像素 的 Sbit RGB 像素 信息 。“c” 代 表 颜 色 ; c:2 代表 红色 ; c:1 4X 


第 5 章 基于 视觉 的 移动 机 器 人 导航 算法 的 实现 示例 123 





KRE; c:0 代表 蓝 色 。 

7) 点 击 “cmdWhiteBlack ”按钮 ， 在 “Shapel” 中 显示 路 径 颜 色 (白色 或 
Am )s 

8) 如 果 根 据 步 又 7 选择 黑色 路 径 ， 则 调用 程序 “Negative”"， 对 采集 图 像 进 
行 反 相 操作 。 然 后 根据 如 示例 6 所 述 的 操作 选择 ( "Optionl" ~ “Option6”) ， 
对 图 像 进行 处 理 。 

图 5.12 给 出 了 “Formnl” 的 布局 。 


cmdWhiteBlack 





œ Lesson? 


Graptlowpass+contrast (^ - : : : ne 
|: | Reference pixel (2,2)-1157.117) ; | | 
SUUxTw no 52 

P Default IE. 


Gray+lowpass+contrast+geometricmean 





图 5.12 “Formnl” 的 布局 
接 下 来 的 文本 显示 了 “cmdWhiteBlack”、“Negative” 和 “Timerl ”的 程序 
代码 ， 其 余 代码 请 参见 示例 6: 
Private Sub cmdWhiteBlack Click( ) 
If sflag = False Then 


sflag = True 
Else 

sflag = False 
End If 


If sflag = False Then Shapel. FillColor = vbWhite 
If sflag = True Then Shapel. FillColor = vbBlack 
End Sub 


Private Sub Negative( width As Long, height As Long) 
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Dim R As Long 
For x=0 To width -1 
For y =0 To height - 1 
R 2255 - Pbytes(2,x,y) ' Invert 
Pbytes(2,x,y) =R 
Pbytes(1,x,y) =R 
Pbytes(0,x,y) =R 
Next y 
Next x 
End Sub 


Private Sub Timerl_ Timer( ) 
Timerl. Enabled = False 
SendMessage hwde, WM CAP GET FRAME,0,0 
SendMessage hwdc, WM CAP COPY,0,0 
Picturel. Picture = Clipboard. GetData 
GetBitmapBits Picturel. Picture , Pinfo. bmWidthBytes * Pinfo. bmHeight, _ 
Pbytes(0,0,0) 
If Optionl. Value = True Then 
Gray Picturel. ScaleWidth. Picturel. ScaleHeight 
If sflag = True Then Negative Picturel. ScaleWidth. Picturel. ScaleHeight 
End If 
If Option2. Value = True Then 
Gray Picturel. ScaleWidth. Picturel. ScaleHeight 
If sflag = True Then Negative Picturel. ScaleWidth , Picturel. ScaleHeight 
Lowpass Picturel. ScaleWidth. Picturel. ScaleHeight 
End If 
If Option3. Value = True Then 
Gray Picturel. ScaleWidth , Picturel. ScaleHeight 
If sflag = True Then Negative Picturel. ScaleWidth. Picturel. ScaleHeight 
Lowpass Picturel. ScaleWidth , Picturel. ScaleHeight 
Contrast Picturel. ScaleWidth , Picturel. ScaleHeight 
End If 
If Option4. Value = True Then 
Gray Picturel. ScaleWidth , Picturel. ScaleHeight 
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If sflag = True Then Negative Picturel. ScaleWidth. Picturel. ScaleHeight 
Lowpass Picturel. ScaleWidth Picturel. ScaleHeight 
Contrast Picturel. ScaleWidth. Picturel. ScaleHeight 
For i = 1 To Val( Text4. Text) 
Geometricmean Picturel. ScaleWidth Picturel. ScaleHeight , gms 
Next i 
End If 
If OptionS. Value = True Then 
Gray Picturel. ScaleWidth Picturel. ScaleHeight 
If sflag = True Then Negative Picturel. ScaleWidth. Picturel. ScaleHeight 
Lowpass Picturel. ScaleWidth Picturel. ScaleHeight 
Contrast Picturel. ScaleWidth, Picturel. ScaleHeight 
For i=1 To Val( Text4. Text) 
Geometricmean Picturel. ScaleWidth , Picturel. ScaleHeight , gms 
Next i 
AdaptiveThreshold Picturel. ScaleWidth , Val( Text2. Text) , Val( Text3. Text) 
End If 
If Option6. Value = True Then 
Gray Picturel. ScaleWidth. Picturel. ScaleHeight 
If sflag = True Then Negative Picturel. ScaleWidth. Picturel. ScaleHeight 
Lowpass Picturel. ScaleWidth. Picturel. ScaleHeight 
Contrast Picturel. ScaleWidth. Picturel. ScaleHeight 
For i=1 To Val( Text4. Text) 
Geometricmean Picturel. ScaleWidth , Picturel. ScaleHeight , gms 
Next 1 
AdaptiveThreshold Picturel. ScaleWidth , Val( Text2. Text) , 
Val( Text3. Text) 
Clean Picturel. ScaleWidth , Picturel. ScaleHeight , Val ( Text3. Text) 
End If 
SetBitmapBits Picture2. Image , Pinfo. bmWidthBytes * Pinfo. bmHeight, _ 
Pbytes(0,0,0) 
Picture2. Refresh 
Picture2. Picture = Picture2. Image 
Picture2. Line( Val( Text2. Text) -2,Val( Text3. Text) -2) . 
— ( Val( Texi2. Text) +2, Val( Text3. Text) +2) ,RGB(255,0,0) ,B 
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Timerl. Enabled = True 
End Sub 
如 果 采 集 图 像 的 大 小 与 图 像框 不 符 ， 那 么 将 激活 “Format” 菜 单 使 得 图 像 大 
小 转换 为 160 x120 像素 。 如 果 没 有 可 用 的 网 络 摄像 头 ， 那 么 会 在 消息 框 中 弹出 
一 条 “未 找到 任何 网 络 摄像 头 ! (No webcam found1)” 的 消息 。 


5.9 示例 8 


目标 : 开发 一 个 VB6 程序 用 于 采集 和 处 理 网 络 视频 流 ， 在 基于 视觉 的 导航 
中 选择 黑色 /白色 路 径 。 根 据 路 径 是 否 存在 来 进行 推理 。 

以 下 步骤 对 程序 开发 过 程 进行 概述 : 

1) 与 示例 7 中 所 述 的 “Webcam7. bas” 相 同 ， 所 有 必需 的 API 调用 均 在 
“Webcam8. pas” 模 块 中 声明 。 同 时 在 VB6 程序 的 “Forml ”中 必须 包含 该 模块 。 

2) AVICAP32. DLL 用 于 通过 调用 合适 的 API 来 采集 网 络 视频 流 。 网 络 摄像 
头 视 频 格式 应 为 RGB24 或 YUY2。 

3) Æ FORMI 中 添加 两 个 “Picture Box ”控件 ，“Picturel1 ”用 于 以 10ms 的 
固定 间隔 从 视频 流 中 采集 图 像 ， 并 将 其 作为 剪贴 板 数据 , “Picture2' JE ' Timerl ' 
控件 下 以 同样 的 速率 对 所 采集 的 图 像 进 行 处 理 。 

4) 在 “Forml” 中 添加 菜单 项 “Format” ， 用 于 设置 图 像 大 小 为 160 x 120 K 
素 。 

5) 通过 调用 “GetObject”API， 从 “Picturel ”中 获得 图 像 像 素数 据 信 息 。 

6) 在 “Timerl ”控件 下 通过 调用 “SGCetBitmapBits” API 获得 一 个 8bit 的 像 
素数 组 “Pbytes (c, x, y)’. “Pbytes” 中 的 每 个 元 素 均 包含 图 像 坐 标 “x ”和 
“y” 处 每 个 像素 的 8bit RGB RAH. 'c' RRB; c:2 代表 红色 ; c:1 代 
表 绿 色 ; c:0 代表 蓝 色 。 

7) 点 击 “cmdWhiteBlack ”按钮 ， 在 “Shapel” 中 显示 路 径 颜 色 (白色 或 
黑色 ) 。 

8) 如 果 根 据 步骤 7 选择 黑色 路 径 ， 则 调用 程序 ‘Negative’, XR E K 
行 反 相 操作 。 然 后 ， 根 据 如 示例 7 所 述 的 “Option6 ” ， 对 图 像 进行 处 理 。 

9) 根据 程序 “WhiteLineWidth” 来 确定 固定 参考 像素 点 [在 (80, 110) 位 
置 处 ] 周围 的 白色 路 径 宽度 。 如 果 参 考 像素 点 周围 的 左 路 径 宽 度 和 右 路 径 宽 度 
小 于 “MIN_PATH_WIDTH” 值 ， 则 推断 出 “No Path' ， 和 否则 得 出 “Path found’ 
的 结论 ， 并 在 文本 框 中 显示 。 

图 5. 13 给 出 了 “Forml” 的 布局 。 

接 下 来 的 文本 显示 了 “Timerl ”的 程序 代码 ， 其 余 代 码 请 参见 示例 7: 
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图 5. 13 *Forml' 的 布局 
Private Sub Timerl Timer( ) 
Timerl. Enabled = False 
SendMessage hwdc,WM_CAP_GET_FRAME,0,0 
SendMessage hwdc, WM_CAP_ COPY ,0,0 
Picturel. Picture = Clipboard. GetData 
GetBitmapBits Picturel. Picture , Pinfo. bmWidthBytes * Pinfo. bmHeight , _ 
Pbytes(0,0,0) 
Gray Picturel. ScaleWidth. Picturel. ScaleHeight 
If sflag = True Then Negative Picturel. ScaleWidth. Picturel. ScaleHeight 
Lowpass Picturel. ScaleWidth. Picturel. ScaleHeight 
Contrast Picturel. ScaleWidth. Picturel. ScaleHeight 
For i=1 To Val( Text4. Text) 
Geometricmean Picturel. ScaleWidth , Picturel. ScaleHeight , gms 
Next i 
AdaptiveThreshold Picturel. ScaleWidth , Val ( Text2. Text) , Val( Text3. Text) 
Clean Picturel. ScaleWidth. Picturel. ScaleHeight. Val( Text3. Text ) 
WhiteLineWidth Picturel. ScaleWidth , Val( Texi2. Text) , Val( Text3. Text) 
SetBitmapBits Picture2. Image , Pinfo. bm WidthBytes * Pinfo. bmHeight, _ 
Pbytes(0,0,0) 
Picture2. Refresh 
Picture2. Picture = Picture2. Image 
Picture2. Line ( Val ( Text2. Text) - 2, Val ( Text3. Text) - 2) - ( Val 
(Text2. Text) - 2, Val( Texi3. Text) +2) ,RGB(255,0,0) ,B 


128 基于 视觉 的 自主 机 器 人 导航 





If PixelCountLeft < MIN PATH WIDTH And PixelCountRight < _ 
MIN PATH WIDTH Then 
Text5. Text 2 " No path " 


Else 
Text5. Text = " Path found " 
End If 
Timerl. Enabled = True 
End Sub 


如 果 和 采集 图 像 的 大 小 与 图 像框 不 符 ， 那 么 将 激活 “Format” 菜 单 使 得 图 像 大 
小 转换 为 160 x 120 像素 。 如 果 没 有 可 用 的 网 络 摄像 头 ， 那 么 会 在 消息 框 中 弹出 
一 条 “未 找到 任何 网 络 摄像 头 ! (No webcam found!)” 的 消息 。 


5.10 示例 9 


目标 : 开发 一 个 VB6 程序 用 于 采集 和 处 理 网 络 视 频 流 ， 在 基于 视觉 的 导航 
中 选择 黑色 /白色 路 径 。 根 据 路 径 是 否 存 在 来 进行 推理 。 应 用 合适 的 规则 来 确定 
不 同 的 导航 方向 以 及 差 动 驱 动 的 速度 参数 。 

以 下 步骤 对 程序 开发 过 程 进行 概述 : 

1) 与 示例 8 中 所 述 的 “Webcam8.bas” 相同 ， 所 有 必需 的 API 调用 均 在 
“Webcam9. bas” 模 块 中 声明 。 同 时 在 VB6 程序 的 “Form1l ”中 必须 包含 该 模块 。 

2) AVICAP32. DLL 用 于 通过 调用 合适 的 API 来 采集 网 络 视频 流 。 网 络 摄像 
头 视频 格式 应 为 RGB24 或 YUY2。 

3) Æ FORMI 中 添加 两 个 “Picture Box ”控件 ，“Picturel ”用 于 以 10ms 的 
固定 间隔 从 视频 流 中 采集 图 像 ， 并 将 其 作为 剪贴 板 数据 ，Picture2 fE ' Timerl ’ 
控件 下 以 同样 的 速率 对 所 采集 的 图 像 进行 处 理 。 

4) 在 “Forml” 中 添加 菜单 项 “Format” ， 用 于 设置 图 像 大 小 为 160 x120 像 
素 。 
5) 通过 调用 “GetObject”API， 从 “Picturel ”中 获得 图 像 像 素数 据 信息 。 
6) 在 “Timerl” 控件 下 通过 调用 “GetBitmapBits ”API 获得 一 个 8bit 的 像 
素数 组 “Pbytes (c，x，y)"。“Pbytes” 中 的 每 个 元 素 均 包含 图 像 坐标 “x” 和 
“y” 处 每 个 像素 的 8bit RGB RARE. ‘ce’ RRB; c:2 代表 红色 ; c:1 代 

RARE; c:0 代表 蓝 色 。 

7) 点 击 “cmdWhiteBlack ”按钮 ， 在 “Shapel” 中 显示 路 径 颜色 (白色 或 
黑色 ) 。 

8) 根据 示例 8 处 理 采 集 图 像 。 如 果 找 到 路 径 ， 则 根据 三 条 规则 确定 合适 的 
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导航 方向 ( ‘forward’ BẸ ‘turn-left’ 2X ‘turn-right’) 和 相应 的 差 动 驱动 速度 参 
数 。 图 像框 显示 导航 方向 。 
K 5.14 给 出 了 “Forml” 的 布局 。 


= Lesson? - Rule based differential drive 





图 5.14 “Forml” 的 布局 
接 下 来 的 文本 显示 了 “Timerl ”的 程序 代码 ， 其 余 代 码 请 参见 示例 8 : 
Private Sub Timerl Timer( ) 
Timerl. Enabled - False 
SendMessage hwdc, WM CAP GET FRAME,0,0 
SendMessage hwde, WM CAP COPY ,0,0 
Picturel. Picture = Clipboard. GetData 
GetBitmapBits Picturel. Picture , Pinfo. bmWidthBytes * Pinfo. bmHeight, _ 
Pbytes(0,0,0) 
Gray Picturel. ScaleWidth , Picturel. ScaleHeight 
If blkflag = True Then Negative Picturel. ScaleWidth, Picturel. ScaleHeight 
Lowpass Picturel. ScaleWidth , Picturel. ScaleHeight 
Contrast Picturel. ScaleWidth , Picturel. ScaleHeight 
For i=1 To Val( Text4. Text) 
Geometricmean Picturel. ScaleWidth Picturel. ScaleHeight , gms 
Next i 
AdaptiveThreshold Picturel. ScaleWidth , Val ( Text2. Text) , Val( Text3. Text) 
Clean Picturel. ScaleWidth , Picturel. ScaleHeight, Val( Text3. Text) 
WhiteLineWidth Picturel. ScaleWidth , Val( Text2. Text) , Val( Text3. Text) 
SetBitmapBits Picture2. Image. Pintb. bn WidthBytes * Pinfo. bmHeight, _ 
Pbytes(0,0,0) 
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Picture2. Refresh 
Picture2. Picture = Picture2. Image 
Picture2. Line ( Val ( Text2. Text) - 2, Val ( Text3. Text) - 2) - ( Val 
( Text2. Text) _ 
+2. Val( Text3. Text) +2) ,RGB(255,0,0) ,B 


If PixelCountLeft < MIN PATH. WIDT H And PixelCountRight < — 
MIN PATH WIDTH Then 
Text5. Text 2 " No path " 
Else 
Text5. Text =" Path found " 
End If 
If PixelCountLeft > = MIN PATH WIDTH And PixelCountRight < _ 
MIN PATH WIDTH Then 
Text6. Text 20: Text7. Text = 50 "Fe fe 
Text8. Text =" Turn left " 
Picture3. Picture = LoadPicture(" turn. left. jpg ") 
End If 
If PixelCountLeft < MIN PATH WIDTH And PixelCountRight > = — 
MIN PATH WIDTH Then 
Text6. Text = 50; Text7. Text 20 ' 右 转 
Text8. Text 2 " Turn right " 
Picture3. Picture = LoadPicture(" turn. right. jpg ") 
End If 
If PixelCountLeft > = MIN PATH WIDTH And PixelCountRight > = _ 
MIN PATH WIDTH Then 
Text6. Text = 100: Text7. Text = 100 ' 向 前 
Text8. Text 2 " Forward " 
Picture3. Picture = LoadPicture( " forward. jpg ") 
End If 
If PixelCountLeft < MIN PATH WIDTH And PixelCountRight < _ 
MIN PATH WIDTH Then 
Text6. Text 20: Text7. Text =0 无 路 径 ,忽略 
Text8. Text = "" 
Picture3. Picture = LoadPicture( " blank. jpg ") 
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End If 
Timerl. Enabled = True 
End Sub 
图 5. 15 所 示 图 像 文件 用 于 表示 导航 方向 。 


Ts ^ 


Forward.jpg turn left jpg turn right.jpg 


图 5.15 图 像 文件 
如 果 采 集 图 像 的 大 小 与 图 像框 不 符 ， 那 么 将 激活 “Format” 菜 单 使 得 图 像 大 
小 转换 为 160 x120 像素 。 如 果 没 有 可 用 的 网 络 摄像 头 ， 那 么 会 在 消息 框 中 弹出 
一 条 “未 找到 任何 网 络 摄像 头 ! (No webcam found!)” 的 消息 。 


5.11 示例 10 


目标 : 开发 一 个 VB6 程序 用 于 采集 和 处 理 网 络 视频 流 ， 在 基于 视觉 的 导航 
中 选择 黑色 /白色 路 径 。 根 据 路 径 是 否 存在 来 进行 推理 ， 应 用 合适 的 规则 来 确定 
不 同 的 导航 方向 以 及 差 动 驱动 的 速度 参数 ， 同 时 添加 声音 输出 以 引起 关注 。 

以 下 步骤 对 程序 开发 过 程 进行 概述 : 

1) 所 有 必需 的 API 调用 均 在 “Webcam10. bas’ 模块 中 声明 。 同 时 在 VB6 
程序 的 “Formnl” 中 必须 包含 该 模块 。 

2) AVICAP32. DLL 用 于 通过 调用 合适 的 API 来 采集 网 络 视频 流 。 网 络 摄像 
头 视频 格式 应 为 RGB24 或 YUY2。 

3) Æ FORMI 中 添加 两 个 “Picture Box” 控 件 ，“ Picturel ”用 于 以 10ms 的 
固定 间隔 从 视频 流 中 采集 图 像 ， 并 将 其 作为 剪贴 板 数据 ,“ Picture2’ 在 “Timerl ’ 
控件 下 以 同样 的 速率 对 所 采集 的 图 像 进 行 处 理 。 

4) 在 “Forml” 中 添加 菜单 项 “Format' ， 用 于 设置 图 像 大 小 为 160 x 120 & 
素 。 

5) 通过 调用 “GetObject”API， 从 “Picturel ”中 获得 图 像 像 素数 据 信 息 。 

6) 在 “Timerl ”控件 下 通过 调用 “GetBitmapBits” API 获得 一 个 8bit 的 像 
素数 组 “Pbytes (c,，x,y)”。“Pbytes” 中 的 每 个 元 素 均 包含 图 像 坐标 “x ”和 
“y” 处 每 个 像素 的 8bit RGB 像素 信息 。“c” 代 表 颜 色 ; c:2 代表 红色 ; c:1 fX 
表 绿 色 ; c:0 代表 蓝 色 。 

7) 点 击 “cmdWhiteBlack ”按钮 ， 在 “Shapel ”中 显示 路 径 颜 色 (白色 或 
黑色 ) 。 


132 基于 视觉 的 自主 机 器 人 导航 





8) 根据 示例 9 处 理 采集 图 像 。 如 果 找 到 路 径 ， 则 根据 三 条 规则 确定 合适 的 
导航 方向 (“forward ”或 “turn-left，” 或 “turn-right”) 和 相应 的 差 动 驱动 速度 参 
数 。 图 像框 显示 导航 方向 。 

9) 调用 程序 “SndPlaySound” 以 及 合适 的 “wave” 文 件 , 来 激活 声音 输出 。 

下 面 给 出 “Webcam10. bas’ 模块 的 程序 : 

Global Const WS_CHILD As Long = &H40000000 

Global Const WS_ VISIBLE AS Long = &H10000000 

Global Const WM _ USER = 1024 

Global Const WM CAP DRIVER CONNECT = WM USER + 10 

G10bal Const WM CAP SET PREVIEW = WM USER +50 

Global Const WM CAP SET PREVIEWRATE = WM USER +52 

Global Const WM CAP DRIVER DISCONNECT As Long WM USER +11 

Global Const WM CAP DLG VIDEOFORMAT As Long = WM USER +41 

Global Const WM CAP DLG VIDEOCOMPRESSION As Long = _ 

WM USER +46 

Global Const WM_CAP_DLG_VIDEODISPLAY As Long = WM_USER +43 

Global Const WM CAP DLG VIDEOSOURCE As Long = WM USER +42 

Global Const WM CAP GET FRAME As Long = 1084 

Global Const WM CAP COPY As Long = 1054 

Global Const WM CAP SET SCALE As Integer = WM USER +53 

Global Const SWP_NOMOVE As Integer =2 

Global Const SWP_NOZORDER As Integer 24 

Global Const HWND_ BOTTOM As Integer = 1 

Global Const SND_ ASYNC =1 

Global Const SND_ LOOP =8 

Global Const SND_ NODEFAULT =2 

Global Const SND_SYNC =0 

Global Const SND_ NOSTOP = 16 

Global Const SND_ MEMORY =4 





Declare Function SendMessage Lib " user32 " Alias " SendMessageA " ( ByVal 
hwnd_ 
As Long, ByVal wMsg As Long, ByVal wParam As Long, ByVal lParam As 
Long) _ 
As Long Declare Function capCreateCaptureWindow Lib " avicap32. dll " 
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Alias . 

" capCreateCaptureWindowA "( ByVal nWindowName As String, ByVal nStyle_ 

As Long, ByVal nx As Integer, ByVal ny As Integer, ByVal nWidth As 
Integer, - 

ByVal nHeight As Integer. ByVal nHwnd As Long, ByVal nld As Integer) As 
Long 


Declare Function SetWindowPos Lib " user32 "( ByVal hwnd As Long, - 
ByVal hWndlnsertAfler As Long, ByVal x As Long, ByVal y As Long, 
ByVal cx As Long, ByVal cy As Long, ByVal wFlags As Long) As Long 
Declare Function GetObject Lib " gdi32 " Alias " GetObjectA " ( ByVal hObject As 
Long, 
ByVal nCount As Long,lpObject As Any) As Long 
Declare Function GetBitmapBits Lib " gdi32 "( ByVal hBitmap As Long, ByVal dw- 
Count - 
As Long,lpBits As Any) As Long 
Declare Function SetBitmapBits Lib " gdi32 "( ByVal hBitmap As Long, ByVal dw- 
Count 
As Long,lpBits As Any) As Long 
Declare Function sndPlaySound Lib " winmm. dll " Alias " sndPlaySoundA " 
(ByVal IpszSoundName As String, ByVal uFlags As Long) As Long 
图 5.16 给 出 了 “Forml” 的 布局 。 
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Al 5.16 *Forml' 的 布局 
接 下 来 的 文本 显示 了 “Timerl ”的 程序 代码 ， 其 余 代码 请 参见 示例 9 
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Private Sub Timerl _Timer( ) 
Timerl. Enabled = False 
SendMessage hwdc, WM CAP GET FRAME,0,0 
SendMessage hwdc, WM CAP COPY ,0,0 
Picturel. Picture = Clipboard. GetData 
GetBitmapBits Picturel. Picture , Pinfo. bmWidthBytes * Pinfo. bmHeight, _ 
Pbytes(0,0,0) 


Gray Picturel. ScaleWidth , Picturel. ScaleHeight 
If blkflag = True Then Negative Picturel. ScaleWidth , Picturel. ScaleHeight 
Lowpass Picturel. ScaleWidth, Picturel. ScaleHeight 
Contrast Picturel. ScaleWidth , Picturel. ScaleHeight 
For i=1 To Val( Text4. Text) 

Geometricmean Picturel. ScaleWidth , Picturel. ScaleHeight. gms 
Next i 
AdaptiveThreshold Picturel. ScaleWidth , Val( Text2. Text) , Val( Text3. Text) 
Clean Picturel. ScaleWidth , Picturel. ScaleHeight , Val( Text3. Text) 
WhiteLineWidth Picturel. ScaleWidth , Val( Text2. Text) , Val( Text3. Text) 


SetBitmapBits Picture2. Image. Pinto. bmWidthBytes * Pinfo. bmHeight, - 
Pbytes(0,0,0) 
Picture2. Refresh 
Picture2. Picture = Picture2. Image 
Picture2. Line ( Val ( Text2. Text) - 2, Val ( Texi3. Text) ~ 2) - ( Val 
( Texi2. Text) _ 
+2. Val( Text3. Text) +2) ,RGB(255,0,0) ,B 


If PixelCountLeft < MIN PATH WIDTH And PixelCOuntRight < _ 
MIN PATH WIDTH Then 
If Text5. Text < >" No path " Then sndPlaySound " No path. wav ", 
SND_ASYNC Or SND_ NODEFAULT 
Text5. Text =" No path " 

Else 
If Text5. Text < >" Path found " Then sndPlaySound " Path found. wav ",_ 

SND ASYNC Or SND NODEFAULT 
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Text5. Text =" Path found " 
End If 


If PixelCountLeft > = MIN PATH WIDTH And PixelCountRight < _ 
MIN_PATH_ WIDTH Then 
Text6. Text =0:Text7. Text =50 “ 左 转 
Text8. Text 2 " Turn left " 
Picture3. Picture = LoadPicture(" turn. left. jpg ") 
End If 
If PixelCountLeft < MIN PATH WIDTH And PixelCountRight > = — 
MIN PATH WIDTH Then 
Text6. Text 250: Text7. Text =0 ‘A, 4% 
Text8. Text =" Turn right " 
Picture3. Picture = LoadPicture(" turn. right. jpg ") 
End If 
If PixelCountLeft > = MIN PATH. WIDTH And PixelCountRight > = _ 
MIN PATH WIDTH Then 
Text6. Text = 100; Text7. Text 2 100 “向 前 
Text8. Text: " Forward " 
Picture3. Picture = LoadPicture( " forward. jpg ") 
End If 
If PixelCountLeft < MIN PATH. WIDTH And PixelCountRight < — 
MIN PATH WIDTH Then 
Text6. Text =0:Text7. Text =0 JG RES , AIK 


Text8. Text = "" 
Picture3. Picture = LoadPicture( " blank. jpg ") 
End If 
Timerl. Enabled = True 
End Sub 


必要 时 ， 通 过 PC 的 声卡 来 播放 两 个 预先 录制 的 声音 文件 “Nopath. wav’ 和 
“Pathfound. wav”。PC 的 声音 录制 程序 可 用 来 创建 这 些 声 音 文件 。 

如 果 采 集 图 像 的 大 小 与 图 像框 不 符 ， 那 么 将 激活 Forma? KAEI ERK 
小 转换 为 160 x 120 像素 。 如 果 没 有 可 用 的 网 络 摄像 头 ， 那 么 会 在 消息 框 中 弹出 
一 条 “未 找到 任何 网 络 摄像 头 ! (No webcam found!1)” 的 消息 。 
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5.12 本章 小 结 


在 此 ， 一步 步 地 介绍 了 十 个 示例 来 展示 在 32 位 Windows 环境 下 ， 视 觉 导航 
应 用 程序 的 开发 技巧 。 

示例 1: 该 示例 演示 了 如 何 从 低 成 本 网 络 摄像 头 的 视频 流 中 采集 图 像 帧 ， 并 

示例 2: 该 示例 演示 了 如 何 处 理 来 自视 频 流 的 采集 图 像 帧 ， 包 括 彩 色 一 灰 度 
图 像 变换 和 低 通 滤波 两 种 处 理 。 

示例 3: 增加 基于 直方 图 拉 伸 技术 的 对 比 度 增 强 方法 ,来 提高 光照 较 差 条 件 
下 的 对 比 度 。 

示例 4: 增加 几何 均值 滤波 器 来 平滑 和 抑制 图 像 细 节 。 

示例 5: 在 光照 变化 条 件 下 ， 引 入 一 个 自 适 应 冰 值 来 提取 白色 路 径 。 

示例 6: 提供 一 个 清除 操作 来 去 除 检测 到 的 不 必要 物体 。 

示例 7: 增加 一 种 路 径 颜 色 选 择 (白色 或 黑色 ) 的 操作 。 

示例 8: 以 一 个 固定 像素 点 为 参考 ， 调 整 导航 中 白色 或 黑色 路 径 的 搜索 。 

示例 9: 引入 一 种 基于 规则 的 方法 来 确定 导航 中 一 个 差 动 驱动 系统 的 左 、 夸 
轮 速 度 设 定 。 

示例 10: 在 导航 过 程 中 ， 增 加 声音 输出 以 引起 注意 。 
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摘要 : 本 章 讨 论 了 如 何 开 发 一 种 基于 视觉 的 导航 方法 来 跟踪 室内 路 径 ， 使 
得 机 器 人 能 够 跟踪 一 条 细 线 或 沿 着 一 条 较 宽 的 路 径 运动 。 在 此 ， 该 方法 中 采用 
模糊 逻辑 来 达到 预期 目标 。 在 前 方 没有 障碍 物 的 情况 下 ， 该 方法 将 基于 模糊 视 
觉 的 导航 方法 来 对 机 器 人 进行 导航 。 另 外 ， 当 检测 到 有 任何 障碍 物 时 ， 该 方法 
还 采用 了 一 种 基于 红外 的 模糊 避 障 策略 。 


6.1 简介 


本 章 将 介绍 一 种 基于 视觉 的 导航 算法 ， 并 结合 实验 室 自主 开发 的 机 器 人 ， 
在 存在 或 没有 障碍 的 情况 下 ， 采 用 模糊 逻辑 来 进行 路 径 跟 踪 ” o EER, EE 
动机 器 人 导航 中 ,模糊 逻 辑 作 为 一 种 可 行 方法 已 被 广泛 接受 。 在 参考 文献 [1] 
中 ， 首 次 开发 了 一 种 用 于 避 障 的 模糊 控制 器 。 其 中 ,控制 器 采用 一 种 基于 视觉 
的 算法 从 摄像 机 中 获取 机 器 人 前 方 隐 人 得 区 域 和 空白 区 域 的 相关 信息 ， 并 利用 仿 
真 器 来 推导 模糊 规则 。 在 参考 文献 [2] 中 的 另 一 项 关于 走廊 导航 的 类 似 研究 工 
作 是 在 所 开发 的 ATHENE 智能 车 上 采用 了 一 个 基于 视频 图 像 的 模糊 控制 器 。 十 
多 年 来 ， 模 糊 逻 辑 方法 广泛 用 于 移动 机 器 人 的 导航 系统 。 参 考 文献 [3] 中 提出 
了 一 种 用 于 移动 机 器 人 控制 的 基于 模糊 逻辑 的 路 径 规 划 和 执行 方法 。 几 乎 同一 
时 期 ， 参 考 文献 [4] 中 提出 的 一 种 用 于 移动 机 器 人 视觉 导航 的 模糊 逻辑 方法 ， 
在 防 撞 和 避 障 方面 得 到 了 成 功 应 用 。 在 参考 文献 [5] 中 ， 提 出 了 一 种 基于 预测 
学 习 模 糊 控制 的 新 方法 ， 其 中 通过 一 个 CCD 摄像 头 来 获取 环境 信息 。 在 该 研究 
工作 中 ， 所 采集 的 图 像 被 分 为 几 种 特征 模板 ， 对 于 每 种 模板 ,根据 控制 规则 集 
对 机 器 人 进行 编程 ， 并 设 为 先 验 知 识 。 通 过 模板 匹配 ， 机 器 人 将 这 些 规则 集 整 
合成 单个 集合 。 另 外 ， 还 有 一 些 研究 工作 是 利用 模糊 控制 器 并 结合 立体 视觉 系 
统 和 一 些 传 统 传感器 来 进行 机 器 人 导航 “”。 在 参考 文献 [8] 中 ， 对 几 种 基于 
模糊 逻辑 的 移动 机 融 人 导航 方法 进行 了 详细 而 全 面 的 介绍 。 近 年 来 ， 一 种 称 为 
基于 规则 的 模糊 通行 性 指数 方法 用 于 移动 机 器 人 的 室外 导航 ， 其 中 通常 根据 图 
像 数 据 进 行 不 同 特征 分 类 ， 如 地 形 粗糙 度 、 地 形 坡度 、 地 形 不 连续 性 以 及 地 形 
人 硬度 等 特征 ”。 一 旦 从 可 视 场景 中 提取 出 这 些 特征 ， 则 可 制订 用 于 移动 机 器 人 
平滑 导航 的 通行 性 指数 模糊 规则 。 

在 此 ， 采 用 第 4 章 所 介绍 的 本 实验 室 自 主 开发 的 机 器 人 ， 并 提出 一 种 基于 
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模糊 逻辑 的 移动 机 器 人 导航 新 方法 。 在 存在 障碍 或 没有 障碍 的 情况 下 ， 该 方法 
能 够 跟踪 罕 路 径 或 宽 路 径 的 中 线 。 机 器 人 前 方 没有 障碍 时 ， 该 系统 采用 一 个 基 
于 视觉 的 模糊 导航 模块 。 一 旦 机 器 人 感知 到 前 方 有 障碍 物 ， 则 停 用 该 模糊 模块 ， 
并 激活 一 个 基于 红外 的 模糊 避 障 模块 ， 以 使 得 机 器 人 能 够 安全 避 障 ， 并 重新 定 
位 在 路 径 的 中 线 上 。 如 果 完 成 该 目标 ， 则 停 用 基于 红外 的 模糊 模块 ， 并 重新 激 
活 基于 视觉 的 模糊 模块 ， 使 得 机 器 人 继续 跟踪 路 径 。 机 器 人 系统 中 采用 了 第 4 
章 中 所 述 的 适合 于 客户 端 /服务 器 操作 的 内 网 连接 功能 ， 从 而 可 正确 选择 机 器 人 
的 功能 ， 并 能 够 在 远 端 客户 端的 PC 对 机 器 人 发 出 合适 的 控制 指令 


6.2 所 提 方 法 概述 


图 6.1 给 出 了 本 书 研 究 工 作 所 开发 的 完整 方案 。 设 在 当前 给 定时 刻 ， 差 动 
驱动 机 器 人 系统 的 位 置 为 (xn，yn，da)。 根 据 机 器 人 前 方 的 环境 ， 对 机 器 人 发 
出 一 个 新 的 导航 命令 ， et 转向 角 指 

开始 





= 


位 于 机 器 人 前 方 的 红外 传感器 扫描 





前 方 是 否 有 障碍 ? 


: ` 执行 基于 红外 的 避 障 














图 6.1 移动 机 器 人 的 导航 策略 
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令 可 以 是 相对 于 机 器 人 当前 位 置 以 道 时 针 方 向 感知 的 (0" ~ 180°) 范围 内 的 任意 
值 。 图 6.2 给 出 了 由 XWY 表示 的 世界 坐标 系 以 及 由 roy 表示 的 移动 机 器 人 坐标 
AR, HP o 为 机 器 人 的 中 心 。 新 的 机 器 人 导航 方向 是 沿 图 6.2 中 op 的 方向 。 在 
任意 给 定位 置 ， 机 器 人 利用 位 于 4、5 和 6 点 处 的 红外 传感器 扫描 前 方 ， 以 确定 
前 方 区 域 是 否 有 障碍 物 。 如 果 检 测 到 障碍 物 ， 机 器 人 将 首先 发 出 一 条 前 方 有 障 
但 的 语音 消息 , 希望 在 听 到 机 器 人 语音 消息 后 ， 可 将 误 放 在 机 器 人 路 径 上 的 障 
得 物 清 除 。 如 果 障 碍 物 没 有 清除 ， 机 器 人 将 根据 在 11 个 扫描 位 置 得 到 的 红外 数 
据 进 行 避 障 。 避 障 策 略 是 机 器 人 应 暂时 偏离 其 原 路 径 ， 成 功 避 障 后 ,仍然 返回 
到 理想 的 原 路 径 。 一 旦 机 器 人 重新 检测 到 前 方 没有 障碍 ， 系 统 将 返回 到 基于 视 
觉 的 导航 控制 机 制 。 所 开发 的 本 系统 在 基于 视觉 的 导航 和 基于 红外 的 避 障 中 都 
采用 一 种 模糊 导航 算法 。 导 航 机 制 的 基本 原理 是 无 论 路 径 上 是 否 具有 障碍 物 ， 
机 器 人 都 应 跟踪 朝向 目标 点 的 一 条 路 径 的 中 线 。 对 于 较 宽 的 路 径 ， 机 器 人 总 是 
沿路 径 中 线 进 行 导 航 。 同 理 ， 对 于 较 窗 的 路 径 ， 即 路 径 宽 度 小 于 机 器 人 本 体 宽 


度 ， 导 航 算法 也 是 试图 跟踪 该 路 径 的 中 线 。 


Y 





Fd 6.2 机 器 人 的 坐标 系 


6.3 基于 视觉 的 机 器 人 模糊 导航 系统 


如 果 机 器 人 发 现 前 方 没有 障碍 ， 将 执行 基于 视觉 的 导航 。 该 导航 机 制 采 用 
了 如 下 图 像 处 理 步 又 : 
1. 从 网 络 摄像 头 的 视频 流 中 采集 一 帧 图 像 
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当 经 过 红外 系统 扫描 推断 出 机 器 人 前 方 区 域 无 障碍 时 ， 在 机 器 人 上 笔记 本 
电脑 网 络 摄像 头 所 获取 的 连续 视频 流 中 采集 一 帧 图 像 ， 该 帧 图 像 中 包含 了 机 器 
人 前 方 环境 的 可 视 化 信息 。 对 该 帧 图 像 进一步 处 理 从 中 提取 有 用 信息 ， 首 先 将 
彩色 图 像 转换 为 相应 的 灰 度 图 像 ， 并 对 该 灰 度 图 像 执 行 图 像 处 理 步骤 

2. 在 环境 的 灰 度 图 像 中 提取 路 径 

接 下 来 是 对 图 像 进行 分 割 ， 使 得 能 够 从 环境 背景 中 提取 出 路 径 。 为 此 ， 首 
先 对 该 图 像 进行 低 通 滤波 来 去 除 品 点 ， 然 后 自动 校正 图 像 亮 度 ， 如 果 由 于 光线 
较 暗 而 造成 图 像 较 黑 ， 则 通过 改变 亮度 值 的 动态 范围 来 增强 图 像 的 整体 亮度 
然后 对 亮度 校正 后 的 图 像 进行 处 理 ， 利 用 一 种 类 似 于 结构 元 素 膨 胀 算法 的 操作 ， 
来 连接 并 加 粗 孤 立 的 亮点 。 这 种 连接 加 粗 操作 可 通过 一 种 几何 平均 滤波 技术 来 
实现 。 

最 后 ， 对 加 粗 后 的 图 像 进 行 闽 值 分 割 。 为 在 环境 背景 中 仅 提 取 路 径 ， 在 此 
选取 较 大 的 亮度 国 值 。 ee D E 
后 ， 没 有 进行 几何 均值 滤波 和 经 过 几何 均值 滤波 后 的 一 个 示例 环境 。 图 6.5 和 
图 6.6 给 出 了 类 似 的 情况 ,只 是 在 路 径 上 有 一 个 障 但 物 。 图 6.6 给 出 图 如 何 利用 
几何 平均 滤波 去 除 图 像 分 割 后 的 干扰 物体 ， 并 清晰 地 提取 路 径 ， 这 在 图 6.5 中 
是 无 法 实现 的 。 








d) e) 


图 6.3 一 个 示例 环境 的 图 像 处 理 结果 
a) 采集 的 原始 图 像 b) 相应 的 灰 度 图 像 “) 去 噪 后 的 图 像 
d) 亮度 自动 校正 后 的 图 像 e) 阅 值 分 割 后 的 最 终 图 像 
3. 采用 基于 视觉 的 模糊 导航 系统 
根据 上 一 步骤 中 所 获得 的 疯 值 图 像 ， 开 发 一 个 基于 模糊 逻辑 的 导航 系统 。 
在 本 例 中 ， 图 像 大 小 为 160 x 120 像素 ， 其 中 左上 人 角 像素 点 作为 坐标 原点 (0， 
0)， 右 下 角 像 素 点 对 应 坐标 (159，119) 。 然 后 在 图 像 的 垂直 中 线 上 选择 一 个 种 
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e) 
Kd 6.4 图 6.3 中 示例 环境 的 图 像 处 理 结 果 


a) 采集 的 原始 图 像 b) 相应 的 灰 度 图 像 “) 去 品 后 的 图 像 d) 亮度 自动 校正 后 
的 图 像 e) 采用 儿 何 均值 滤波 进行 白 立 点 连接 与 加 粗 后 的 图 像 浆 值 分 割 后 的 最 终 图 像 








d) e) 








图 6.5 具有 干扰 物体 的 示例 环境 的 图 像 处 理 结果 

a) 采集 的 原始 图 像 b) 相应 的 灰 度 图 像 “) KRE KER 

d) 亮度 自动 校正 后 的 图 像 e) 阔 值 分 割 后 的 最 终 图 像 
子 点 S$， 该 种 子 点 越 朝 向 图 像 底 部 ， 表 明 该 点 实际 位 置 对 应 于 越 靠 近 机 器 人 的 实 
际 环境 点 。 在 图 像 坐 标 系 中 ， 该 种 子 点 选 为 《80，110) 。 在 该 点 位 置 处 在 图 像 
中 绘制 一 sie 从 该 种 子 点 S 开始 ， 可 向 左 和 向 右 来 遍历 该 扫描 线 ， 由 此 
计算 这 些 具 有 亮度 值 的 像素 点 个 数 (包括 5 点 左 侧 和 右 侧 的 所 有 像素 点 )， 从 而 
确定 机 器 人 左 侧 和 右 侧 的 路 径 宽度 。 如 果 两 侧 的 像素 点 个 数 相同 ， 即 可 推断 机 








142 基于 视觉 的 自主 机 器 人 导航 








e) 


图 6.6 具有 干扰 物体 的 示例 环境 的 图 像 处 理 结果 
a) 采集 的 原始 图 像 b) 相应 的 灰 度 图 像 c) 去 噪 后 的 图 像 d) 亮度 自动 校正 后 的 图 像 
e) 采用 几何 均值 滤波 进行 孤立 点 连接 与 加 粗 后 的 图 像 f) 阅 值 分 割 后 的 最 终 图 像 

避 人 大 致 位 于 路 径 的 中 线 上 。 田 一 方面 ， 如 果 左 侧 像 素 点 个 数 大 于 右 侧 像素 点 
个 数 ， 则 表明 机 器 人 位 置 偏向 路 径 的 右 侧 ， 然 后 模糊 推 新 系统 就 试图 使 机 器 人 
ashen 如 果 右 侧 像素 点 个 数 大 于 左 侧 像素 点 个 数 ， 则 表明 机 器 人 偏 
向 路 径 左 侧 ， 然 后 模糊 系统 就 给 机 器 人 发 出 相反 命令 ， 使 得 机 器 人 向 路 径 中 线 
运动 ， tt tir ac eh 其 中 两 个 输 

人 变量 为 PixelCountLeft 和 PixelCountRight， 两 个 输出 变量 分 别 为 线 速度 指令 (v) 
和 转向 角 指 令 (9) 。 所 开发 的 模糊 系统 是 一 种 零 阶 Takas-Sugano(TS) 系 统 。 为 使 
得 该 模糊 系统 的 鲁 棒 性 更 强 ， 将 种 子 点 左 侧 和 右 侧 的 像素 个 数 取 为 在 三 个 种 子 
点 Si=(80，109) 、S$ =(80，110) 和 3$:=(80，111) 处 绘制 的 三 条 连续 的 水 平 
线 ， 然 后 计算 其 平均 值 为 








3 
PixelCountLeft = zo pel, (6. 1a) 
PixelCountRight = => per, (6. 1b) 
i=l 
AFP pel, 种 子 点 S, 沿 扫描 线 向 左 的 像素 个 数 ; 
per, 种 子 点 s, 治 扫描 线 向 右 的 像素 个 数 。 





图 6.7 给 出 了 模糊 化 时 的 输入 隶属 函数 ， 其 中 每 个 输入 变量 都 用 小 (S)、 中 
(M) 和 大 (L) 三 种 隶属 函数 模糊 化 。 输 入 变量 的 模糊 集 或 隶属 函数 分 别 表 示 为 
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图 6.7 PixelCountLeft( 见 图 6. 7a) 和 PixelCountRight( 见 图 6. 7b) 的 隶属 函数 
s,, — PixelCountLeft 


S 





) sp < PixelCountLeft < s, 
cl 一 Sb 


从 ,( PixelCountLeft ) = 0<PixelCountLeft<s,, (6.2a) 


其 他 
PixelCountLeft — ms) 








m, -m, m, < PixelCountLeft < m, 
- Patent m, € PixelCountLeft x m 
eb 7 My 


| 
by CPixelCountLeft) = (^: ma 
à 其 他 


(6. 2b) 
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| —— Shi la < PixelCountLeft < Ly 
Mi ( PixelCountLeft) = i fi PixelCountLeft > L, (6. 2c) 
: 其 他 
E e sp, < PixelCountRight < s,, 
cr =s r 
从 .( PixelCountRight) = : 0 < PixelCountRight x s,, 
其 他 
(6. 3a) 
[mme 
kom, = m,, < PixelCountRight < m,, 
Hy ( PixelCountRight) = 一 ea m, < PixelCountRight x m,, 
CP 其 他 
(6. 3b) 
a Zhe l, < PixelCountRight < /,, 
pı ( PixelCountRight) = j o 2 PixelCountRight > /,, 
] 其 他 


(6. 3c) 
对 于 每 个 输出 变量 ， 输 出 由 单 变 量 表 示 。 模 糊 规 则 库 由 一 组 IF-THEN 模糊 
见 则 组 成 。 一 条 模糊 规则 i 可 表示 为 
RO: IF x, 是 Ma x, 是 Ma 
THEN y, 是 Vu an 和 Xy» 是 O8 ns i=l, 2, =, N (6.4) 
式 中 x=[x， x, | = [ PixelCountLeft , PixelCountRight ] ' ; 
yix, hy)" = [tess 8,1: 
M, e {S,M,L}; 
M, e IS,M,Li; 
€ Vaiss 
0 5 € Oasi 
= 输出 线 速 EARE = [v vl» Vyi29 U’ vanl; 
0... = 输出 转向 角 单 矢量 = [6 Oiio 0, x Owl. 
N 一 一 模糊 规则 库 中 规则 的 总 数 。 


a 
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表 6.1 视觉 导航 系统 的 模糊 规则 库 





















































— Bü fF (IF 子 句 ) l 后 件 (THEN F4) 
PixelCountLeft PixelCountRight zs( 标 么 值 ) 0,;,/° 
1 小 小 0.9 90 
2 小 中 0.5 | 67 
3 小 K 0.1 45 
4 中 小 0.5 112 
5 中 中 0.9 90 
6 中 夫 0.5 67 
7 类 小 0.1 135 
8 大 中 0.5 112 
9 大 大 0.9 90 
所 构建 的 模糊 规则 库 见 表 6. 1。 线 速度 的 模糊 输出 为 一 个 标 么 值 ， 该 值 是 乘 
以 一 个 合适 的 增益 (K,,。,)， 通 过 采用 加 权 平 均 法 来 去 模糊 化 ， 然 后 输出 线 速度 
指令 (v,,) 和 输出 转向 角 指令 (9, ) 计 算 如 下 : 
bi vas * ax) 
va = (use =| (6.5) 
| 2, a(x) 
S Oy: *a(x) 
B, = = — (6.6) 
> a(x) 
式 中 a, (x) 规则 i = [[ u(x) 的 触发 强度 。 


4. 如 果 机 器 人 偏离 路 径 则 保存 可 能 的 绕 行 转向 角 

机 右 人 导航 系统 中 配置 了 一 个 附加 模块 来 应 对 紧急 情况 。 考 虑 到 在 某 些 环 
境 下 ， 机 器 人 偏离 了 路 径 ， 并 且 在 处 理 后 的 输出 图 像 中 ， 变 量 PixelCountLeft 和 
PixelCountRight 均 计 算 为 零 。 在 这 种 情况 下 ， 以 迭代 方式 给 定 机 器 人 一 个 很 小 的 
转向 角 绕 行 命令 ( 线 速度 取 为 零 ) ， 直 到 在 变量 PixelCountLeft 和 PixelCountRight 
中 ， 至 少 有 一 个 不 为 零 。 由 此 可 推断 机 器 人 已 朝向 原 路 径 ， 因 此 随后 激活 的 基 
于 视觉 的 导航 算法 将 试图 使 机 器 人 返回 到 路 径 的 中 线 。 现 在 ， 机 器 人 应 按 顺 时 
针 方 向 还 是 按 逆 时 针 方 向 绕 行 取决 于 机 如 人 在 之 前 的 迭代 过 程 中 是 朝 左 运动 还 
是 朝 右 运动 。 这 有 助 于 减少 绕 行 时 所 花费 的 时 间 ， 并 能 够 恢复 导航 的 原 方向 。 

图 6.8 给 出 了 一 种 保存 可 能 转向 角 绕 行 信 息 的 算法 。 在 每 个 采样 时 刻 (%)， 
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通过 一 次 水 平 扫描 来 计算 种 子 点 左 侧 像素 个 数 (pcli ) HUE BUR I CC per), A 
而 确定 亮点 的 个 数 。 为 保证 机 器 人 转向 正确 ， 该 过 程 要 重复 执行 (N_rows) 次 来 
确定 在 采样 时 刻 上 的 累积 值 cum. pel, 和 cum_per。 在 基于 视觉 导航 的 每 帧 处 理 
图 像 中 重复 执行 该 过 程 ， 以 确定 在 采样 时 刻 % 所 保存 的 两 个 相应 量 的 最 后 值 。 
然而 在 保存 这 些 值 时 ， 当 前 采样 时 刻 的 优先 级 最 高 ， 随 着 返回 过 去 时 刻 ， 相 应 
的 优先 级 也 逐步 减 小 。 这 可 由 下 式 表示 : 

store pcl, = k, x cum pcl + k, x store pel, , (6.7) 

store per, = k, x cum pcr, + k, X store pcr, , (6.8) 

在 该 系统 中 ， 遗 忘 因子 取 为 上 0.25 LE, =0.75。 若 基于 视觉 的 导航 算法 
正常 运行 ， 则 继续 保存 。 然 而 只 有 当 由 于 某 些 原因 ， 机 器 人 偏离 路 径 且 变量 Pix- 
elCountLeft 和 PixelCountRight 为 零 时 ， 这 些 保存 值 才 开始 有 用 。 接 下 来 ， 根 据 
(store_pel,_store_per,) 的 极 性 ， 选 择 有 效 的 转向 绕 行 方向 。 

如 果 值 为 正 ， 则 选择 逆 时 针 绕 行 方 向 ， 否 则 选择 顺 时 针 绕 行 方向 。 一 旦 绕 
行 方 向 固定 ， 就 执行 迭代 过 程 。 在 此 ， 机 器 人 的 线 速度 为 零 ， 固 定 旋转 10*. R 
集 一 帧 图 像 ， 并 按 之 前 所 述 的 图 像 处 理 步 又 进行 处 理 , 来 确定 变量 Pixel- 
CountLeft 和 PixelCountRight 的 值 。 如 果 至 少 一 个 值 不 为 零 ， 则 重新 激活 基于 视觉 
的 导航 算法 ,否则 机 器 人 旋转 10" 并 执行 后 续 步 又 。 和 迭代 执行 上 述 处理 过 程 ， 直 
到 重新 激活 基于 视觉 的 导航 算法 。 


6.4 基于 红外 传感器 的 模糊 避 障 算法 


如 果 系 统 检测 到 前 方 有 障碍 ， 则 激活 基于 红外 的 避 障 模块 ， 并 停 用 视觉 系 
统 。 一 个 基于 模糊 逻辑 的 红外 避 障 机 制 将 使 得 机 器 人 绕 开 障碍 并 继续 沿 原 路 径 
运动 。 一 旦 机 器 人 避 开 障碍 ， 则 重新 激活 基于 视觉 的 导航 算法 ， 这 将 使 得 机 器 
人 自动 返回 到 路 径 的 中 线 。 对 于 所 开发 的 红外 模糊 系统 ， 每 个 红外 传感器 在 11 
个 角度 (1L=1，2，…，11) 进 行 扫 描 ， 从 而 产生 11 个 红外 读数 IR. Sensor val 
(1) ， 单 位 为 伏 (V)。 这 11 个 读数 分 为 三 组 ， 即 IR_Group_val(p), p=1, 2, 
3。 这 样 做 是 为 了 减少 模糊 系统 的 输入 维 数 。 在 每 个 红外 传感器 组 中 ， 选 取 最 大 
扫描 读数 作为 该 组 的 读数 。 因 为 读数 越 高 ， 表 明 障 碍 越 近 。 因 此 ， 这 些 模拟 组 
读数 可 表示 为 

IR_Group_val(1) = max(IR. Sensor val(1) |] = 1,2,3,4) (6.9) 
IR. Group. val(2) = max(IR. Sensor. val(/) |] = 5,6,7) (6. 10) 
IR. Group. val(3) = max(IR Sensor val(1) |] = 8,9,10,11) (6.11) 

接 下 来 ， 开 发 一 个 三 输入 两 输出 的 模糊 避 障 系统 ， 其 中 三 个 输入 为 IR_ 

Group_val(P),，DP=1，2，3， 两 个 输出 为 (>z，6) 。 在 此 还 开发 了 一 个 零 阶 TS 模 
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row=0; IX=80; IY=110 


选择 种 子 点 作为 (IX, TY) 并 通过 水 平 
扫描 来 计算 pcli (LX. 1Y) 和 per, (1X. TY) 












cum pcl, = cum pcl,*pcl, (1X, TY) 
cum pcr, cum_per,+per, (IX, IY) 













store pcl, = k,xcum_pcel,+k)xstore_pely.; 
Store pcr,— kixcum_ pcrj-k»*store per, 





图 6.8 可 能 转向 角 绕 行 模块 的 信息 存储 过 程 
糊 系 统 。 图 6.9 给 出 了 对 每 个 输入 变量 所 选择 的 隶属 函数 ， 每 个 输入 采用 三 个 
隶属 函数 ， 即 远 (FR)、 中 (IM) 和 近 ( NR ) 来 模糊 化 。 相 应 的 隶属 函数 可 表示 为 


F, <IR_Group_val(p) < F, 





F,-lR. Group. val(p) 
Fa E Fn 
Bg (IR_Group_val(p)) = OxIR Group val(p)&F,, 


1 
其 他 


(6. 12a) 
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c) 


图 6.9 隶属 函数 


b)IR. Group. val(2) 


p. val(3) 


IR_ Grou 


c) 


Group. val(1) 


a) IR 
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p 


P.L. I, «IR. Group. val(p) «1, 





je val(p) - I, 





Lm(lR_ Group. val(p) ) = (2 一 Ty I. «IR. Group. val(p) «1, 





Is -Ip 其 他 
0 

(6. 12b) 

IR. Group. val(p) - N,, 

Na -N, 
Las (IR. Group. val(p)) = 
0 
N,, «IR. Group val(p) « Np 

IR. Group. val(p) 三 No (6. 12c) 


其 他 
模糊 规则 i 可 表示 为 
Rin: IF z 是 0 和 于 是 0 和 2 是 0a 
THEN u, Æ Voas aM u Æ Oois 2, i21, 2, cc, L (6.13) 
AP z= [z，z,， a = [ IR_Group_val (1), IR_Group_val(2), IR_ 

Group_val(3) ]'; 
u=(u,, i] = aus Bu] 3 

Q, e | FR,IM,NR| ; 

Q, e |FR,IM,NRI; 

Q4 e | FR,IM,NRI| ; 


Vu, à € Vas 
05. E 0,.5 
va. = 输出 线 速度 单 矢 量 = [vw Yon st Voy]; 
Ou = 输出 转向 角 单 矢量 = [6 ,go Oaa] 5 
[一 一 该 模糊 系统 的 规则 总 数 。 
d 6.2 给 出 了 避 障 时 的 整个 模糊 规则 库 。 设 Ka ,为 线 速 度 的 比例 增益 ， 输 
出 线 速度 指令 (vw, ) 和 输出 转向 角 指 令 (0,,,) 可 用 下 式 计算 : 


L 


vas * Bi (Z) 
1 


Pa. = (Kei obs) x = L 
2 B.G) 


obs 


(6. 14) 
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L 
2 Oops; * B.(z) 


























































































































9.45 T (6.15) 
> BG 
i=l 
3 
jel 
He 6.2 避 障 时 的 模糊 规则 库 
规 前 件 (IF 子 句 ) 后 件 (THEN 子 句 ) 
则 
DL IR. Gourp. val(1) | IR. Gourp. val(2) | IR. Gourp. val(3) vas CER A TEL) 04.7 (?) 
1 FR FR FR 0.8 0 
l« ss 
2 FR FR IM 0.8 90 
3 FR FR NR 0.7 90 
4 FR IM FR 0.5 135 
5 FR IM IM 0.7 135 
6 FR IM NR 0.6 150 
7 FR NR FR 0.3 135 
dl | 
8 FR NR IM 0.5 135 
$$$ 
9 FR NR NR 0.4 160 
10 IM FR FR 0.9 90 
11 IM FR IM 0.8 90 
12 IM FR NR 0.6 90 
= 
13 IM IM FR 0.7 50 
14 IM IM | IM 0.3 90 
15 IM IM NR 0.2 105 
16 IM NR FR 0.5 35 
17 IM NR IM 0.2 105 
18 IM NR NR 0.1 150 
19 NR FR FR 0.8 90 
RACE e 
20 NR FR IM 0.7 90 
+ 
21 NR | FR NR 0.6 | 90 
22 NR IM FR 0.5 40 
23 NR IM IM 0.2 25 
24 NR IM NR 0.1 90 
25 NR NR FR 0.4 30 
+ 4. 
26 NR NR IM 0.1 15 
27 NR NR NR 0 90 
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6.5 实际 性 能 评估 


利用 本 书 所 提 系 统 在 真实 室内 环境 下 进行 了 几 个 实验 ， 在 此 主要 介绍 四 种 
示例 情况 。 

1. As fil I 

在 本 示例 中 ， 要 求 机 器 人 沿 一 条 曲线 运动 。 路 径 宽度 小 于 机 器 人 本 体 的 宽 
度 。 图 6. 10 给 出 了 机 器 人 执行 该 任务 时 的 序列 图 像 。 图 6. 10a ~ 上 分 别 给 出 了 机 
器 人 试图 跟踪 路 径 中 线 的 图 像 序列 。 对 于 所 有 示例 情况 ， 采 用 视觉 和 红外 测 上 距 
的 导航 ， 因 此 在 客户 端 关 闭 接近 觉 传感器 。 图 6. 11 给 出 了 细 实 线 表示 的 完整 运 
动 路 径 。 机 器 人 的 理想 路 径 用 路 径 中 线 上 的 点 线 表 示 。 由 图 可 知 ， 机 器 人 的 实 
际 运动 路 径 与 理想 路 径 非 常 接近 。 只 是 在 角 点 处 ， 实 际 路 径 与 理想 路 径 的 偏差 
稍 大 。 这 不 难 理解 ， 在 采用 基于 模糊 的 导航 算法 时 ， 实 际 机 器 人 应 经 过 一 个 平 
滑 的 转向 角 。 





e) 


图 6.10 示例 I 中 机 器 人 经 过 的 路 径 

2. adn I 

在 本 示例 中 ， 要 求 机 器 人 沿 一 条 折线 运动 ， 当 路 径 结 束 时 ， 大 概 是 个 U É, 
并 沿路 径 返 回 到 其 初始 起 始点 。 图 6. 12 给 出 了 机 器 人 执行 该 任务 的 图 像 序列 。 
该 示例 研究 证 明 当 前 向 路 径 结束 时 ， 从 远程 客户 端 对 机 器 人 发 出 利用 所 保存 的 
可 能 转向 角 绕 行 信息 指令 来 自动 转弯 ， 并 沿 原 路 径 返 回 。 因 此 ， 在 到 达 路 径 终 
点 时 ， 机 器 人 无 需 停 止 ，PixelCountLeft 和 PixelCountRight 计算 得 到 零 值 。 实 际 
上 ， 机 器 人 以 一 种 和 欠 代 方式 不 断 转 弯 ， 直 到 其 能 重新 跟踪 路 径 。 图 6. 12a ~ 上 分 
别 给 出 了 机 器 人 前 向 运动 时 试图 跟踪 路 径 中 线 的 序列 图 像 。 
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0 15 30 45 60 75 90 105 120 135 150 


x 坐 标 /cm 
图 6.11 示例 工 中 机 器 人 经 过 的 完整 路 径 
图 6. 12g ~k 给 出 了 机 器 人 以 迭代 方式 执行 转弯 操作 ， 使 其 能 重新 定位 路 径 
中 线 时 的 序列 图 像 。 图 6. 121~。o 给 出 T Bude A HT 重新 跟踪 路 径 并 返回 其 原始 起 











图 6.12 示例 五 中 机 器 人 经 过 的 路 径 
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m) n) 0) 


图 6.12 示例 下 中 机 器 人 经 过 的 路 径 ( 续 ) 
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E 前 向 响应 
35 

一 - — 反 向 响应 
30 


理想 路 答 


4 坐标 /cm 





0 10 20 30 40 50 60 70 80 90 
x 坐标 /cm 
图 6.13 示例 了 中 机 器 人 经 过 的 完整 路 径 
台 点 的 序列 图 像 。 从 客户 端 选 择 “Search ON” 操作 可 使 cee 图 重新 跟踪 路 
径 ， 即 便 在 视觉 感知 器 视 场 中 路 径 已 消失 时 。 图 6.13 给 出 了 运动 的 前 向 路 径 
( 实 线 ) 。 机 器 人 的 理想 路 径 为 沿路 径 中 线 的 点 线 。 可 ihe 人 实际 运行 的 路 
= enun IE Rede, fEfügu4b. SE PR ER He PAU eA —EÉím22. Ob, 
Wi Ahan UES ZZ, HAM KR 19 OL tt Ade ye ee S ZT Be on Ir) nu mnl 
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运动 时 的 路 径 有 较 小 偏差 。 这 表明 对 于 机 器 人 前 向 和 返回 运动 时 都 具有 展 好 性 


能 








e) 





图 6.14 示例 焉 中 机 器 人 经 过 的 路 径 
3. n ff TI 
在 本 示例 中 ， 要 求 机 器 人 尽 可 能 地 沿 比 机 咒 人 较 宽 的 路 径 中 线 运 动 。 在 路 
径 上 有 一 个 障碍 物 ， 机 器 人 需要 避 开 。 图 6. 14 给 出 了 机 器 人 如 何 执行 任务 的 序 
列 图 像 。 在 本 实验 中 ， 要 求 机 器 人 在 从 客户 端 以 "Search OFF "操作 下 进行 导航 ， 


实际 响应 








0 15 30 45 60 75 90 105 120 135 150 165 180 
x 坐标 /cm 


图 6.15 示例 焉 中 机 顺 人 经 过 的 完整 路 径 
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图 6.16 示例 NV 中 机 器 人 经 过 的 路 径 

因此 ， 机 器 人 在 成 功 避 障 后 到 达 路 径 终点 时 停止 。 正如 要 求 所 述 ， 当 摄像 头 视 
场 中 路 径 消 失 时 ， 机 器 人 无 需 在 路 径 中 重新 定位 自己 。 图 6.15 给 出 了 细 实 线 所 
示 的 完整 运动 路 径 。 这 表明 机 器 人 如 何在 开始 时 利用 视觉 感知 沿路 径 中 线 运动 ， 
然后 当 遇 到 障碍 物 时 ， 利 用 红外 避 障 算法 左 转 ， 与 障碍 物 几乎 平行 地 成 功 避 障 ， 
然后 当 绕 过 障 但 后 ， 试 图 利用 视觉 感知 来 重新 定位 自身 在 路 径 中 线 。 

4. 示例 IV 

在 本 示例 中 ， 要 求 机 器 人 执行 更 困难 的 任务 ， 机 器 人 必须 沿路 线 两 个 几乎 
垂直 的 转弯 ， 并 要 求治 路 径 中 线 。 在 该 示例 中 ， 要 求 机 器 人 在 从 客户 端 以 
“Search OFF "操作 下 进行 导航 。 图 6. 16 给 出 了 当 机 融 人 执行 导航 任务 时 的 图 像 
序列 。 由 图 可 看 出 尽管 是 直角 转弯 ， 机 器 人 仍 能 够 在 转弯 后 重新 定位 自身 在 路 
径 中 线 ， 并 以 满意 的 形式 沿路 径 运 动 。 从 图 6. 17 中 可 看 出 ， 甚 至 经 过 两 个 直角 
转弯 机 器 人 还 能 够 快速 重新 定位 在 路 径 中 线 ， 实 际 路 径 与 理想 路 径 之 间 的 偏差 
相当 小 。 该 偏差 在 两 个 直角 拐角 处 稍 大 ， 这 再 次 证 明了 之 前 所 述 的 逻辑 
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ss 理想 路 径 


1 坐标 /cm 





0 15 30 45 60 75 90 105 120 
x 坐标 /cm 


图 6.17 示例 NV 中 机 器 人 经 过 的 完整 路 径 
6.6 本 章 小 结 


本 章 讨论 了 在 室内 环境 下 如 何 实现 一 种 简单 而 有 效 的 基于 模糊 逻辑 的 路 径 
跟踪 机 制 ， 在 本 实验 室 自 主 开发 的 移动 机 器 人 上 进行 了 实验 。 该 机 制 是 在 机 器 
人 前 方 无 障碍 时 ,采用 基于 视觉 的 模糊 导航 ; 当前 方 有 障碍 时 ， 采 用 基于 红外 
的 模糊 避 障 ， 同 时 停 用 视觉 导航 机 制 。 一 旦 完成 避 障 ， 则 停 用 红外 避 障 ， 继 续 
采用 视觉 导航 。 不 管 目 标 是 治 着 一 条 细 线 还 是 一 条 较 宽 的 路 径 运动 ， 该 机 制 总 
是 试图 引导 机 器 人 沿 着 路 径 的 中 线 运 动 。 本 系统 能 够 实现 在 有 障碍 和 无 障碍 下 
的 导航 ， 并 成 功 尝试 了 内 网 连接 功能 。 该 移动 机 器 人 路 径 跟 踪 系 统 ， 结 合 本 实 
验 室 自主 开发 的 移动 机 带 人 平台 上 ， 在 几 种 真实 的 室内 环境 下 进行 了 实验 验证 。 
在 此 ， 讨 论 了 四 种 实际 示例 来 表明 该 系统 的 有 用 性 和 有 效 性 。 

致谢 本章 所 介绍 的 工作 得 到 了 印度 大 学 教育 资助 委员 会 重大 人 研究 项 目 计 
划 的 资助 [资助 号 : 32-118/2006 (SR) |. 
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第 7 章 移动 机 器 人 的 SLAM 


摘要 : 本 章 首先 介绍 了 移动 机 器 人 导航 中 的 同时 定位 与 地 图 构建 (SLAM ) 概 
念 ， 然 后 详细 介绍 了 基于 扩展 卡尔 曼 滤 波 (EKF) 的 SLAM 算法 。 接 下 来 ， 在 一 
个 较为 复杂 的 场景 中 ， 且 传感器 数据 不 正确 的 情况 下 来 实现 基于 EKF 的 SLAM 
算法 ， 并 讨论 了 如 何 利用 模糊 或 神经 一 模糊 监督 方法 来 提高 这 种 情况 下 的 估计 
性 能 。 在 此 背景 下 ， 还 讨论 了 如 何 利 用 进化 优化 策略 来 自动 学 习 该 神经 一 模糊 
监督 器 的 自由 参数 。 


7.1 简介 


过 去 的 20 年 中 ，SLAM 问题 在 自主 车 辆 和 移动 机 器 人 等 研究 领域 已 得 到 广 
泛 关 注 。 从 本 质 上 来 说 ，SLAM 问题 是 指 在 一 个 未 知 环境 中 迭代 估计 一 个 移动 平 
台 的 未 知 运动 ， 由 此 来 确定 包含 特征 (也 称 为 路 标 ) 的 环境 地 图 ， 并 在 各 自信 息 
的 基础 上 ， 确 定 移动 平台 的 绝对 位 置 '  。 这 是 一 个 非常 复杂 的 问题 ， 因 为 车 辆 
的 位 姿 估 计 (pose estimate) 及 其 相关 的 地 图 估计 会 由 于 缺少 任何 全 局 位 置信 息 而 
导致 越 来 越 不 精确 ” 。 若 车 辆 没有 配置 全 球 定位 系统 (GPS) ， 就 会 产生 该 问题 。 
因此 ，SLAM 问题 的 复杂 性 是 多 方面 的 ， 由 于 车 辆 位 姿 和 地 图 估计 之 间 的 相互 依 
赖 关 系 ， 这 需要 在 高 维 空间 中 进行 求解 。 

解决 SLAM 问题 的 一 种 最 早 也 是 最 常用 的 方法 是 采用 EKF 技术 。 直 到 现在 ， 
还 有 大 量 研究 工作 是 采用 EKF 方法 来 解决 SLAM 的 相关 问题 E 
PIP 、 室 外 、 水 下 中 、 地 下 中 等 环境 ,已 成 功 开 发 了 一 些 SLAM 算法 。 一 
种 基于 EKF 的 方法 是 在 环境 地 图 中 以 一 种 完整 状态 矢量 的 形式 来 估计 和 保存 机 
器 人 位 姿 和 特征 位 置 ， 并 以 误差 协 方差 矩阵 的 形式 保存 这 些 估 计 的 不 确定 性 。 
这 些 协 方差 矩阵 中 还 包括 了 表示 特征 /路 标 估 计 之 间 互 相关 性 的 元 素 。 然 而 ， 在 











O ”本章 是 基于 : 

1 ) Amitava Chatterjee 和 Fumitoshi Matsuno 于 2007 年 10 月 在 IEEE Transactions on Fuzzy systems 期 刊 上 第 
15 卷 第 5 期 984 ~ 997 页 上 发 表 的 “A neuro-fuzzy assisted extended Kalman filter-based approach for Sim- 
ultaneous Localization and Mapping problems” 论 文 ; 

2) Amitava Chatterjee F 2009 年 5 月 在 Robotica 期 刊 上 第 27 卷 第 3 期 411 ~423 页 上 发 表 的 ”Differential 
evolution tuned fuzzy supervisor adapted extended kalman filtering for SLAM problems in mobile robots" o ££ 
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第 7 章 移动 机 器 人 的 SLAM 159 








采用 经 典 的 完全 基于 EKF 的 SLAM 方法 时 ， 一 个 主要 问题 是 在 具有 大 量 特征 的 
情况 下 ， 计 算 量 会 显著 增 大 ， 这 是 由 于 总 的 状态 矢量 和 总 的 协 方差 矩阵 会 急剧 
增 大 。 之 后 ， 在 对 基于 EKF 的 SLAM 算法 的 改进 研究 中 ,将 该 问题 看 作 一 个 主 
要 的 研究 内 容 ， 目 前 已 提出 一 些 改进 算法 ””” ”。 与 基于 EKF 的 SLAM 算法 相 
关 的 另 一 个 关键 问题 是 由 于 地 图 中 一 些 形 状 相 似 的 路 标 所 造成 的 数据 关联 问题 。 
在 这 种 情况 下 ,不同 的 数据 关联 假设 都 会 产生 多 种 完全 不 同 的 地 图 ， 并 且 无 法 
利用 高 斯 分 布 来 表示 这 种 多 模 态 的 分 布 。 该 问题 通常 是 在 给 定 当 前 机 器 人 地 图 
的 条 件 下 ， 在 单 次 测量 "或 多 次 测量 ”的 基础 上 通过 限制 算法 只 与 最 相似 的 数 
据 进 行 关联 来 解决 。 其 中 ,采用 多 次 测量 的 方法 更 具有 和 鲁 棒 性 。 尽 管 日 前 已 开 
发 出 其 他 一 些 如 参考 文献 [21，22] 中 的 数据 关联 算法 ， 但 由 于 这 些 算法 无 法 实 
时 实现 ， 因 此 意义 不 大 。 

另外 还 提出 了 一 些 解决 SLAM 问题 的 其 他 方法 ， 这 些 方法 都 是 通过 某 些 数 
值 算法 来 实现 的 ， 而 不 是 采用 类 似 于 EKF 中 的 严格 统计 方法 。 其 中 ， 某 些 方法 
基于 贝 叶 斯 理论 ， 这 样 就 无 需 EKF 中 的 假设 条 件 ( 即 不 确定 性 需 由 高 斯 分 布 进行 
建 模 ) 。 采 用 Sequential Monte Carlo( SMC) 方法 开发 出 几 种 这 样 的 算法 ， 其 中 需 
要 进行 粒子 滤波 ”i。 通 过 在 EKF 算法 中 对 非 线性 模型 线性 化 来 引入 一 个 不 
确定 性 ， 即 可 不 用 粒子 滤波 技术 。 然 而 ， 在 基于 粒子 滤波 的 方法 中 ， 和 希望 利用 
大 量 粒子 以 使 得 能 够 包含 一 个 在 每 个 采样 时 刻 与 车 辆 /机 器 人 所 处 位 置 非常 接近 
的 粒子 二 。 如 何 开 发 一 种 有 效 的 SLAM 算法 ， 并 在 粒子 滤波 中 采用 足够 少 的 粒 
子 数 ， 仍 是 目前 一 个 重要 的 研究 内 容 。 在 该 研究 方向 上 ，FastSLAM1.0 和 
FASTSLAM2. 0 算法 是 一 个 重要 的 飞跃 ， 已 成 功 解决 基于 粒子 滤波 的 SLAM 中 的 
维 数 问题 ”。 男 外 ， 还 有 一 些 SLAM 算法 采用 扫描 匹配 技术 得 到 成 功 地 开发 应 
用 ， 即 通过 参考 帧 之 间 的 一 个 空间 关系 图 来 有 效 地 构建 地 图 。 

之 前 已 表明 ，EKF 的 处 理性 能 很 大 程度 上 取决 于 过 程 噪声 协 方差 矩阵 (C ) 
和 测量 噪声 协 方差 矩阵 (R) 的 准确 性 。Q 和 R 的 先 验 知识 不 正确 可 能 会 导致 性 
能 下 降 ” ， 甚 至 可 能 会 导 臻 发散” 。 因 此 ， 自 适应 估计 这 些 和 矩阵 对 于 在 线 配置 
非常 重要 。 在 参考 文献 [28 ] 中 ，Mehra 提出 了 一 种 对 卡尔 曼 滤 波 算法 中 噪声 协 方 
AME O 和 RR 自 适应 估计 的 开创 性 研究 ， 该 方法 是 基于 能 够 对 Q 和 R 进行 渐进 
正 态 、 无 偏 一 致 性 估计 的 创新 相关 性 分 析 方 法 '” ， 具 体 算法 是 基于 一 种 噪声 概 
率 统计 固定 不 变 且 所 考虑 的 模型 是 一 个 时 不 变 模 型 的 假设 。 后来， 在 这 一 研究 
方向 上 也 进行 了 一 些 研究 工作 , 采用 的 是 一 些 自 适应 合计 Q 和 R 的 经 典 方 
法 '””” 。 在 参考 文献 [30] 中 ， 提 出 了 一 种 迭代 算法 和 随机 通 近 算法 相 结合 的 方 
法 来 估计 Q 和 RR。 在 参考 文献 [32] 和 [33] 中 ， 将 问题 扩展 到 对 O 和 R 的 时 变 佑 
计 。 参 考 文献 [34] 对 参考 文献 [28] 中 的 实际 应 用 进行 了 详细 阐述 。 

在 最 近 10 年 ， 还 提出 了 一 些 基于 模糊 或 模糊 神经 网 络 技术 的 自 适应 EKF 算 
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法 “- ”。 在 参考 文献 [38] 中 ， 通 过 采用 一 个 模糊 神经 网 络 确定 ARMA 模型 中 每 
个 工作 点 的 AR 参数 ， 然 后 对 系统 的 等 效 状 态 空间 表示 进行 卡尔 曼 滤波 ， 来 求解 
一 个 输入 一 输出 的 映射 问题 ， 其 中 输出 变量 会 受 测 量 噪声 的 影响 。 在 参考 文献 
[36] 中 ， 利 用 模糊 逻辑 来 同时 自 适 应 估计 O 和 R。 在 参考 文献 [37] 中 ， 对 于 卡 
尔 曼 滤波 算法 ， 利 用 模糊 逻辑 来 仅 估计 丸和 矩阵 。 在 实际 情况 下 ， 无 法 准确 已 知 
这 些 以 2 和 RR 和 矩阵 形式 存在 的 信息 和 矩阵 是 相当 敏感 的 。 这 样 就 会 在 很 大 程度 上 
影响 SLAM 问题 的 性 能 。 

本 章 首 先 详细 介绍 基于 EKF 的 随机 SLAM 算法 。 接 下 来 ,将 探讨 无 法 准确 
已 知 传感器 噪声 统计 信息 情况 下 的 SLAM 问题 。 在 这 些 情况 下 ， 将 讨论 在 模糊 
神经 网 络 的 作用 下 ， 基 于 EKF 的 算法 是 如 何 得 到 有 效应 用 的 “* 。 在 此 ， 详 细 
介绍 了 如 何 利用 一 个 模糊 神经 网 络 模型 来 协助 基于 EKF 的 SLAM 算法 在 每 次 迭 
代 过 程 中 自 适应 地 估计 和 矩 阵 。 另 外 ， 还 将 讨论 如 何 利 用 常用 的 进化 优化 算法 
[如 粒子 群 优化 (PSO ) 算 法 ”和 差分 进化 算法 ] 来 学 习 模 糊 神 经 网 络 模型 中 的 自 
由 参数 。 本 章 所 讨论 的 模糊 自 适 应 EKF 算法 基本 上 可 实现 一 个 更 加 复杂 的 系统 ， 
与 之 前 的 系统 相 比 ， 主 要 表现 在 两 个 方面 : 

1) 对 于 SLAM 问题 ， 由 于 状态 矢量 和 协 方差 矩阵 的 大 小 本 质 上 是 时 变 的 ， 
因此 这 种 情况 基本 上 是 非常 复杂 的 。 这 是 因为 在 导航 过 程 中 ,不 同时 刻下 在 状 
态 矢量 中 初始 化 新 的 路 标 ( 在 某 些 特殊 条 件 下 ， 一 些 已 存在 的 路 标 可 能 会 被 去 
除 ) ， 由 此 造成 这 些 矢量 和 和 矩阵 的 大 小 会 不 断 变化 。 通 常情 况 下 ， 这 些 和 矩阵 的 大 
小 会 逐渐 增 大 。 

2) 本 章 所 讨论 的 方法 是 采用 了 一 种 自学 习 模 糊 神经 网 络 模型 的 通用 方法 。 
这 与 之 前 开发 的 手动 选择 模糊 系统 参数 的 系统 形成 鲜明 对 比 。 

本 章 对 这 些 SLAM 算法 进行 了 详细 深入 的 分 析 。 在 此 ， 针 对 各 种 不 同 的 环 
境 条 件 ( 即 特征 /路 标点 的 个 数 发 生变 化 以 及 一 些 无 法 准确 获知 的 测量 噪声 统计 
值 ) ， 给 出 了 实验 结果 。 


7.2 基于 EKF 的 随机 SLAM 算法 


1. 假设 条 件 

1) 假设 所 考虑 的 特征 均 为 二 维 点 特征 ; 

2) 假设 这 些 特 征 均 为 静态 特征 ， 即 在 所 建立 的 地 图 中 ， 特 征 位 置 不 会 随时 
间 变 化 ; 

3) 在 输入 控制 量 、 转 向 角 控制 指令 (s) 和 后 轮 驱 动 速度 (w) 中 存在 不 确定 
性 ， 并 且 这 些 不 确定 性 可 由 高 斯 分 布 进 行 建 模 ; 

4) 假设 机 器 人 的 起 始 位 姿 是 确定 的 ; 
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5) 在 两 个 连续 采样 时 刻 的 间隔 内 , tise i 是 线性 的 ; 

6) 在 距离 (7r) 测 量 和 朝向 (09) 测量 中 具有 不 确定 性 ， 同 样 这 些 不 确定 性 也 由 
高 斯 分 布 进行 建 模 ; 

7) 这 些 特征 属性 只 有 其 二 维 位 置 ， 在 此 不 考虑 其 他 属性 ， 如 形状 等 。 

2. 算法 

现在 对 采用 EKF 算法 的 基于 特征 地 图 的 SLAM 问题 进行 简要 概述 。 对 该 算 
法 的 详细 阐述 可 参见 参考 文献 16, 7]。 这 些 过 程 是 在 通过 非 线 性 动力 学 约束 和 / 
或 涉及 非 线 性 测量 关系 的 情况 下 ， 可 采用 EKF 算法 来 进行 状态 估计 的 。 在 该 方 
法 中 ， 对 于 根据 测量 所 得 的 状态 估计 连续 更 新 的 滤波 器 估计 轨迹 进行 线性 化 。 
利用 一 个 非 线性 函数 所 ， ) 对 状态 转移 进行 建 模 ， 同 时， 状态 的 观测 值 或 测量 值 
也 可 由 一 个 非 线 性 函数 h(， ) 建 模 ， 如 下 所 示 : 


X, Efx, u,) +4, (7.1) 
Z,,1 HACK) HP ga (7.2) 


式 中 x, REIN Al SERI (n x1) 个 过 程 状态 矢量 ; 
Zz 一 一 采样 时 刻 上 时 的 (m x ) 个 测量 矢量 ; 











随机 变量 ， 分 别 表 示 高 斯 白 噪声 和 测量 噪声 ， 已 Q, RI R, 分 别 表 
IR x, q, 和 产 的 协 方差 矩阵 。 
在 SLAM 问题 中 ， 状 态 矢 量 x 由 车 辆 状态 x, 和 路 标 状态 xu 组 成 。 因 此， 对 


以 均值 矢量 x 形 式 表示 的 总 的 状态 矢量 以 及 相应 的 总 的 误差 协 方差 矩阵 己 的 估 
计 为 


q, Flr, 


wala, Hel (7.3) 
P, Pon 
Pols P | POM 


式 中 x, — 机 器 人 /车 辆 状态 的 均值 估计 (由 其 位 次 表示 ) ; 
P,—  *jx, 相关 联 的 误差 协 方差 矩阵 ; 
fr ,一 特征 位 置 的 均值 估计 |; 
P, —— Sx, 相关 联 的 误差 协 方差 矩阵 。 
在 一 个 任意 直角 坐标 系 中 定义 机 器 人 /车 辆 位 姿 。 认 为 特征 或 路 标 是 一 个 二 
维 的 点 特征 。 假 设 在 地 图 中 观测 到 个 这 样 的 静态 点 特征 ， 则 
t= (Re Kis pl" (7.5) 
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c Try, Tre, 
2 2 2 
P,=| Oy, Oyy, C. (7.6) 
2 2 
Op Tie, T, e 
A & 人员 aK oi 
Xa = [a Ieee, | (1) 
2 2 
c T, c, c 
1*1 1*1 i 1 
2 2 2 2 
T 0,, Cr c 
171 pnl 1 1 
Pa=| : : AC ci : (7.8) 
2 2 
c c... c c 
1 T 
2 2 ae 2 2 
Tuy, 9 y, O x,y 9 y. 


根据 这 些 静 态 特 征 的 位 置 估计 来 定义 地 图 ， 并且 式 (7.4) 的 P,, 中 保持 了 机 
器 人 与 地 图 之 间 的 关系 。P, 中 的 非 对 角 线 元 素 体 现 了 互相 关 性 ， 因 此 特征 本 身 
之 间 的 信息 是 无 关 的 。 假 设 系 统 初 始 时 没有 观测 特征 ， 直 角 坐 标 系 与 机 器 人 的 


起 始 位 姿 一 致 ， 并 且 机 器 人 的 起 始 位 姿 不 存在 不 确定 性 。 用 数学 公式 可 表示 为 x 
=x,=0 和 P=P,=0。 

随 着 机 器 人 开始 运动 ，x, MP, 也 变 为 非 零 值 。 在 随后 的 迭代 中 ， 进 行 第 一 
次 观测 时 ， 要 求 对 新 特征 初始 化 ， 并 首次 出 现 x。 和 P,。 这 就 会 增 大 x 和 P 的 大 


小 ， 并 重新 计算 矢量 zx 和 矩阵 PATER. REAR BG EE 

3. 时 间 更 新 (“ 预 测 ”) 步 又 

在 此 ， 假 设 在 机 器 人 运动 的 影响 下 ,输入 控制 撩 量 w 包括 两 个 控制 输入 ， 
即 转向 角 指令 (s) 和 后 轮 驱动 速度 (w)。 因 此 ,wu = [ws]"。 这 样 ， 状 态 估计 就 可 
通过 编码 器 里 程 计 和 机 器 人 运动 模型 获得 。 在 此 ， 必 须要 考虑 输入 控制 量 w 和。 
的 不 确定 性 (如 由 于 车 轮 打滑 、 控 制 器 校正 不 准 等 引起 的 不 确定 性 ) JEDE w 和， 
的 额定 值 来 进行 一 种 改进 的 高 斯 分 布 建 模 。 因 此 ， 在 预测 步骤 中 从 采样 时 刻 
到 (k+1) 的 间隔 内 计算 状态 估计 的 预测 值 和 误差 协 方差 的 估计 值 ， 即 


Yk+l 


£,, 1) 


x, -f(n.u)- (7.9) 














x m 
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Vf, P NF gt Vf Ui Vf VP 
P= ， (7.10) 
C Vf, P und P 


m 


其 中 ， 在 运动 模型 和 输入 控制 量 的 基础 上 ,用 有 来 估计 机 器 人 位 姿 。 根 据 在 此 
采用 的 移动 机 器 人 的 里 程 计 方 程 ， 即 假设 机 器 人 的 增 量 运动 本 质 上 是 线性 的 ， 
这 样 人 可 表示 为 


x, +w, X At x cos(s, + Qu) 











£o ye hU hh) = y, +w, X At x sin( ss +9,,) (7.11) 
^ ^ sin( s, ) 
Pu, Q,, tw, XAtx WB 
AP WB 一 一 机 器 人 的 轴 距 ; 
Ai 一 一 采样 时 刻 。 
HORIEK EERE, U, A 的 协 方差 矩阵 可 记 为 
a gh 
Vf. = d | Ci, un) (7.12) 
vk ox, k 
af, os o 
Vion au, | (eye) (7.13) 
o 0 
U= (7.14) 
0 o 


在 此 ， 由 于 假设 这 些 特征 均 为 静态 特征 ， 因 此 式 (7.9) 和 式 (7. 10) 中 的 x。 
和 P, 为 时 不 变 常数 。 

4. 测量 值 更 新 (“校正 " ) 步 又 

在 此 ， 假 设 已 观测 到 特征 地 图 中 的 一 个 特征 ， 其 位 置 表示 为 第 i 个 特征 ， 即 
(x,,y,)。 对 于 所 考虑 的 系统 ”” ,假设 是 利用 一 个 二 维 激光 测 距 扫描 仪 (SICK 
PLS) 来 观测 特征 ， 该 激光 测 距 仪 是 目前 在 移动 机 器 人 导航 中 最 常用 的 测 距 传 感 
器 。 假 设 安装 在 移动 机 器 人 前 部 防磁 装置 上 的 该 激光 测 距 扫描 仪 能 够 以 0.5° 的 
间隔 返回 在 180* 平 面 上 扫描 得 到 的 距离 值 ， 这 种 测 距 传感器 的 精度 通常 大 约 为 
+50mm。 值 得 注意 的 是 ， 还 需 假设 移动 机 器 人 上 已 配置 有 车 轮 和 转向 编码 器 。 
表示 每 个 特征 与 激光 扫描 仪 ( 即 移动 机 器 人 ) 之 间 相对 距离 的 距离 测量 值 是 以 极 
坐标 形式 显示 的 。 设 特征 是 由 其 相对 于 观测 者 的 距离 (r) 和 角度 (6) 来 表征 的 ， 
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记 为 
z -[r0]" (7.15) 
这 些 观测 值 中 的 不 确定 性 也 是 由 一 种 改进 的 高 斯 分 布 来 建 模 的 。 设 R 为 相 
应 的 观测 /测量 噪声 协 方差 矩阵 ， 记 为 








c. 0 
r- | (7. 16) 
0 oc, 
在 此 ， 假 设 距离 测量 值 和 和 角度 测量 值 之 间 没 有 互相 关 性 。 在 地 图 中 ,这 些 
测量 值 可 记 为 
/EE + eH," 
z, =, (x,) = AY a (7.17) 
cd: x *. 


现在 ,假设 z 和 (x,，y,) 正 确 关联 ， 由 此 可 计算 EKF 的 增益 W,， 相 应 的 计 
算 公式 为 


Ya TAa china) (7.18) 
S, = Vh, Pri Vh, +R, (7.19) 
Wai =P,,,VA,, Si), (7. 20) 





式 中 ov, 第 i 个 路 标的 观测 新 息 (in novation) ; 
S$ 一 一 关联 新 息 的 协 方 差 矩 阵 。 
TEK IEVA, 可 表示 为 
oh 
Vhs 
k+l OX , k+l 
因此 ， 后 验 增 广 状态 估计 和 相应 的 协 方差 矩阵 可 按 如 下 公式 进行 更 新 : 
Xia =X,,,+W v (7.22) 


thet tha 


(7.21) 


Pj = Pig TW, Su We (1,23) 


在 此 ， 需 要 注意 的 是 ， 除 了 过 程 和 测量 中 的 不 确定 性 ，EKF 算法 中 线性 化 
处 理 也 会 产生 不 确定 性 。 通 过 对 非 线 性 函数 0) RI RC * ) 在 状态 平均 点 处 进行 
线性 化 可 得 到 "时间 更 新 "方程 和 "测量 更 新 "方程 。 具 体 的 线性 化 处 理 是 通过 泰 
勒 级 数 展开 并 忽略 高 阶 项 来 进行 的 。 这 种 通过 一 阶 偏 导 对 非 线 性 系统 进行 近似 
化 处 理 的 方式 会 在 EKF 算法 中 引入 额外 的 不 确定 性 。 实 际 上 ， 对 于 高 阶 非 线性 
函数 ， 这 种 线性 化 处 理 方 式 并 不 能 充分 准确 地 近似 协 方差 转 换 ， 这 也 许 会 导致 
非常 不 一 致 的 不 确定 性 估计 。 在 这 种 情况 下 ， 无 迹 变 换 或 许 能 提供 更 准确 的 结 
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5. 初始 化 新 特征 并 删除 旧 特 征 

在 执行 递归 预测 和 更 新 的 迭代 过 程 中 ， 随 时 都 可 能 观测 到 新 特征 。 然 后 ， 
应 通过 将 这 些 新 特征 位 置 的 二 维 坐标 集成 到 增 广 状态 矢量 中 来 将 其 初始 化 到 系 
统 ， 并 相应 地 改变 协 方差 矩阵 。 由 激光 扫描 仪 观测 到 的 特征 可 能 是 点 、 线 、 角 
点 和 边 等 。 在 此 ， 仅 考虑 这 些 特 征 为 点 特征 时 的 情况 ， 每 个 点 特征 都 表示 二 维 


环境 地 图 中 的 一 个 叭 一点。 按照 参考 文献 [7] 中 的 数学 计算 公式 ， 这 些 新 的 xz 和 
Pj 可 计算 为 


* 
xi = (7.24) 
FAX, Z) 
; 
P, P, P Vf, 
Pe Poy P. Pas Via (7.25) 


" s 
Wi P, VSP em, Vf, P, V fos, + Vf, RY fe 


在 此 , f,(x,，z) 是 将 极 坐 标 观测 值 z SEHR BN ELINA RAF WEA LA DE T 
计算 为 


f, 
Vf, 3 | (x, 5%) 
vk 


af 2 
Vf, = 和 | (Xz) (7.26) 


删除 不 可 靠 的 特征 相对 简单 。 仅 需 从 状态 矢量 中 删除 相关 的 行 矢 量 ， 并 从 
协 方差 矩阵 中 删除 相关 的 行 撩 量 和 列 矢 量 即 可 。 

现在 ,在 执行 观测 步骤 时 ， 同 一 时 刻 多 个 路 标 可 见 是 非常 普遍 的 ， 因 此 需 
要 进行 几 次 独立 的 观测 。 在 本 系统 中 ,假设 执行 一 次 观测 就 可 得 到 一 批 这 样 的 
观测 值 ( 即 z=[7,，0,，…，r,，0,] )， 并 进行 批量 更 新 。 参 考 文献 [7] 表 明 对 
于 SLAM FE, MRP AK TE v 中 同时 包含 多 个 观测 ， 则 EKF 算法 具有 更 好 的 
更 新 性 能 。 在 此 ， 与 参考 文献 [7] 的 论述 一 致 。 因 此 ， 在 批量 模式 的 观测 和 更 新 
过 程 中 ， 相 应 的 SLAM 算法 是 基于 v、S 和 W 矢量 /和 矩阵 的 组 合 ， 并 且 这 三 个 矢 
量 / 和 矩阵 的 大 小 是 随时 间 变 化 的 ， 这 是 因为 在 任意 观测 时 刻 ， 可 见 路 标的 总 个 数 
是 不 断 变化 的 。 
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7.3 结合 模糊 神经 网 络 模型 的 基于 EKF 的 SLAM 算法 


目前 ， 自 适应 EKF 领域 中 的 大 多 数 研究 工作 都 主要 关注 于 利用 新 息 序列 中 
新 的 统计 信息 来 校正 状态 估计 。 本 书 中 所 采用 的 自 适应 EKF 方法 是 基于 一 种 新 
息 自 适应 估计 (IAE) 方 法 。1AE 方法 最 初 在 参考 文献 [28 | 中 提出 ， 随 后 在 参考 文 
献 [37] 中 与 模糊 逻辑 相 结 合 来 应 用 。 该 方法 的 基本 思想 是 首先 确定 任意 第 上 个 
时 刻 的 新 测量 值 x 与 相应 预测 估计 值 z 之 差 ， 然 后 利用 该 信息 来 校正 已 完成 的 
估计/ 预测 。 自 适应 策略 的 目标 是 减少 新 息 序 列 (5, ) 的 理想 协 方差 与 相应 的 实际 


协 方差 Ci 之 间 的 不 一 致 。 在 本 书 的 SLAM 算法 中 ， 根 据 式 (7.19) 计 算 S,, E 
中 等 式 右边 符合 批 处 理 观测 和 更 新 的 思想 。C, 可 计算 为 


C oca (7.27) 

AP v, 符合 批量 模式 的 增 广 新 息 序 列 。 
根据 参考 文献 137] ， 应 根据 在 一 个 合适 的 运动 估计 窗口 (大 小 为 W) 中 对 w， 
2 运动 平均 来 计算 协 方差 。 然 而 对 于 SLAM 问题 ， 增 量 w ,的 大 小 随时 间 不 断 变 
化 ， 这 是 由 于 应 取决 于 任意 一 个 观测 和 更 新 步骤 中 之 前 至 少 观测 到 一 次 的 路 标 


个 数 。 因 此 ， 利用 式 (7.27) 来 计算 C，,， 而 不 是 采用 一 种 运动 平均 。 由 此 ， 可 
计算 在 第 个 时 刻 的 不 一 致 为 


A C, = Cy 7S, (7.28) 


在 此 ， 目 标 采用 模糊 逻辑 来 最 小 化 该 不 一 致 ， 通 过 对 A C,，, 和 矩阵 中 每 个 对 
角 元 素 都 采用 一 个 单 输入 单 输出 的 模糊 神经 网 络 系统 来 实现 。 利 用 这 些 模糊 规 


则 来 调整 R 矩阵 ， 使 得 传感器 统计 适应 于 不 一 至 矩阵 A t. 中 的 减 小 。 一 个 结 
合 模糊 神经 网 络 的 基于 EKF 的 完整 SLAM 算法 如 算法 7.1 所 示 。 设 系统 中 连续 
输入 控制 信号 的 采样 时 间 为 25ms。 
算法 7.1 结合 模糊 神经 网 络 的 基于 EKF 的 SLAM 算法 
由 算法 7.1 可 知 ， 每 个 NFS 都 采用 一 种 非 线 性 映射 形式 : ARG, j) 2f us A 


Cio Gis DO. Feb ARG. 门 对 应 于 增 广 测量 噪声 协 方差 矩阵 R 中 相应 对 角 元 素 
的 推荐 值 ， 并 根据 批量 模式 计算 。 在 每 次 迭代 执行 观测 和 更 新 步骤 时 计算 该 增 
广 矩 阵 ， 其 大 小 取决 于 观测 步 又 中 所 观测 到 的 总 的 路 标 个 数 。 为 保证 与 之 前 至 
少 观 测 过 一 次 的 批量 路 标 一 致 ， 该 增 广 和 矩阵 R 的 大 小 为 [2z x2z] ， 其 中 2, 为 在 
本 次 迭代 中 所 观测 到 的 路 标 数 。 利 用 初始 [2 x2]R 矩阵 来 构成 该 增 广 矩阵 及， 
可 表示 为 
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0 ov; 0 0 
: 0 
R=|i : 0 oF i : (7.29) 
0 Q co :… o gd 0 
0 Qf e sm ee X) c, 


HEP, o 和 cs 对 应 于 本 次 迭代 所 计算 的 传感器 统计 值 。 由 此 可 看 出 ， 增 广 
和 矩阵 R 仅 由 对 角 元 素 组 成 ， 而 所 有 非 对 角 元 素 都 为 零 。 这 与 7.2 节 中 的 假设 相 
一 致 ， 即 距离 测量 值 和 方位 测量 值 相互 独立 ， 之 间 没 有 互相 关 性 。 在 不 同 的 迭 
fep, HJ BEER 的 大 小 不 断 变 化 ， 这 是 因为 ， 在 某 次 迭代 过 程 中 ， 之 前 已 访 
问 过 现 又 被 观测 到 的 路 标 个 数 会 随 着 迭代 的 执行 而 变化 。 增 广 和 矩 阵 R 的 维 数 与 


Ci 以 及 A Ci. 一致。 

根据 对 增 广 和 矩阵 R 中 每 个 对 角 元 素 都 采用 相同 的 NFS 来 实现 的 思想 ， 对 每 
个 NFS 都 采用 归 一 化 的 输入 。 在 实际 中 ，NFS 采用 下 列 三 种 形式 的 模糊 IF- 
THEN 规则 : 


IF(A Coas 站) 为 负 THENAR(j, j) =w, 
IF(A Cw(j, 门 ) 为 零 。 THENAR(j, j) =w,， 


IF(AC, LU, DIDE  THENAR(), j) em, 
w, w, 和 w, 表示 AR 矩阵 中 对 角 元 素 的 模糊 适应 度 推荐 值 ， 这 取决 于 


A Cu 矩阵 中 相应 对 角 元 素 模 糊 化 不 一 致 的 特性 。 然 而 对 于 距离 观测 和 方位 
观测 ， 不 匹配 的 顺序 可 能 会 不 同 ， 这 取决 于 分 别 已 知 的 距离 观测 和 方位 观测 
传感器 统计 值 的 不 准确 性 (或 准确 性 ) 程 度 。 因 此 ,在 按照 算法 7.1 正确 计 


算 A Cua ee sina 和 A Cun_usu 的 基础 上 ， 分 别 采用 距离 观测 和 方位 
观测 的 归 一 化 输入 。 然 后 利用 这 些 归 一 化 输入 ，NFS 可 计算 每 个 对 角 元 素 的 
AR(j, j) 。 最 后 计算 适应 度 ， 即 初始 [2 x 2] R 矩阵 所 需 的 Ao? 和 Ai， 在 均值 
正确 的 基础 上 ， 分 别 计算 AR(j, 让 数组 中 距离 观测 和 方位 观测 的 相关 元 素 。 当 
在 执行 EKF 中 的 观测 和 更 新 步 又 ， 并 将 用 于 随后 形成 的 增 广 矩阵 及 等 情况 下 ， 
在 下 一 次 迭代 中 ,调整 后 的 初始 矩阵 [2 x2]R 已 准备 好 。 然 后 ， 如 果 在 观测 步 
又 中 观测 到 新 特征 时 ， 则 通过 采用 式 (7.24) ~ 式 (7.26) 来 增 广 状 态 矢量 和 相应 
的 协 方差 矩阵 ， 从 而 执行 每 次 观测 和 更 新 的 步 又 。 


168 基于 视觉 的 自主 机 器 人 导航 





1. IF 已 经 过 所 有 导航 点 ，THEN 停止 ENDIF, 
2. 计算 机 器 人 与 当前 导航 点 之 间 的 距离 。 
IF( 距 离 < 所 允许 的 最 小 距离 ) ， 
THEN 切换 下 一 个 导航 点 作为 当前 导航 点 ENDIF, 

3. 计算 与 当前 导航 点 的 转向 角 变化 量 (As)， 然 后 得 到 新 的 转向 角 (s)[ 满足 
最 大 转向 角 变 化 率 ( As,,. ) 和 最 大 转向 角 (s,,, ) 的 约束 条 件 ] 。 

4. 机 器 人 运动 并 确定 其 实际 位 姿 。 

5. 按照 式 (7.7) ~ 式 (7.10) ， 执 行 EKF 预测 步骤 。 

6. IF( Time. for Observation Jj E), THEN 跳 转 到 步骤 7，ELSE 跳 转 到 步骤 
1, ENDIF, 

7. 确定 从 机 器 人 当前 的 实际 位 置 所 观察 到 的 可 见 路 标 集合 。 计 算 每 个 路 标 
的 实际 距离 值 和 方位 角 。 根 据 已 观测 到 的 路 标 和 新 观测 到 的 路 标 ， 分 别 进行 观 
i) 。 

8. 在 第 5 步 预 测 的 总 状态 矢量 增 量 的 基础 上 ， 对 步骤 7 中 已 观测 到 的 路 标 
预测 距离 一 方位 值 。 

9. 在 实际 和 预测 值 的 基础 上 ， 采 用 式 (7.14)， 对 已 观测 到 的 路 标 批 处 理 计 
算 增 广 新 息 序 列 (v)。 

10. 批 处 理 计 算 相 应 的 增 广 测量 噪声 协 方差 矩阵 R( 利 用 原始 [2 x 2] R RR) 
和 增 广 线性 观测 模型 h。 

11. ÆR RA h 的 基础 上 ， 利 用 式 (7. 15 ) 批 处 理 计算 增 广 5S。 

12. 根据 式 (7. 18) 和 式 (7.19)， 更 新 后 验 状态 估计 矢量 和 误差 协 方差 矩阵 。 


13. 根据 式 (7.23) 和 式 (7.24) ,分别 计算 Ci 和 A Ci,， 并 确定 A Ci 的 大 
小 ， 即 [A Cs, A Condo 

14. 分 别 确定 A Ci 矩阵 相应 对 角 线 元 素 的 距离 观测 误 匹 配 的 最 大 绝对 什 
(A Ci e cna) 和 方位 观测 误 匹 配 的 最 大 绝对 值 (A Cis searing sinn 0 

15. FOR 对 于 AC.j=1, 

用 合适 的 A Cua ranee miman BÈ A Cu 上 归 一 化 相应 的 对 角 线 元 素 
A Cua. D. 


确定 NFS 输出 的 相应 ARC), j) DURAS ATH M46 A Cau Gis J)o 
ENDFOR 

16. 将 Ao? 作为 对 应 于 距离 测量 的 AR(j, j) 元 素 的 均值 。 

17. 将 Mo? 作为 对 应 于 方位 测量 的 AR(j, j) 元 素 的 均值 。 
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Ao? 0 
18. 原始 [2 REE R =R + | 
0 Ag, 


19. IF( 在 步骤 7 中 观测 到 新 特征 ) 

THEN 根据 式 (7.20) ~ 式 (7.22) ， 计 算 增 广 状 态 矢 量 和 误差 协 方差 矩阵 。 
ENDIF 

20. 返回 步骤 1。 


7.4 模糊 神经 网 络 架构 及 基于 粒子 群 优化 (PS9O ) 算法 的 训 
练 方法 


7.4.1 模糊 神经 网 络 架 构 


所 开发 的 模糊 神经 网 络 模型 是 一 个 单 输入 单 输出 的 系统 。 四 层 架 构 如 图 7.1 
Pitas. BE ul 和 0! 分 别 表 示 第 1 层 中 第 i 个 节点 的 输入 和 输出 。 


AC LJ) 





图 7.1 模糊 神经 网 络 系统 的 四 层 架 构 ( 经 IEEE 许可 ， 从 参考 文献 [44] 中 复制 ) 
1. 第 一 层 : 输入 层 
该 层 只 包括 一 个 节点 ， 表 示 单 输入 变量 。 该 节点 的 输入 、 输 出 关系 为 


0' =u =A Cual, j) (7.30) 

2. 第 二 层 : 隶属 函数 层 
在 该 层 中 ,输入 变量 通过 三 个 隶属 函数 (MF) ， 即 负 (N) 、 零 (Z) 和 正 (P) 进 
行 模糊 化 处 理 。 图 7. 2 给 出 了 这 三 个 隶属 函数 ， 其 中 N, 和 N, 分 别 表示 隶属 了 
BON BEDARA S, Zu Za ZM Z ,分 别 表示 隶属 函数 Z 的 左 基点 、 左 


DA., ADANAN, P, 和 P, 分 别 表示 隶属 函数 P. 的 左 基点 和 左 顶 点 。 第 i 
个 隶属 函数 的 输出 为 
OF =p,(u') =p;(A Cua. 2) (7.31) 


3. 第 三 层 : 去 模糊 化 层 
该 层 执行 去 模糊 化 操作 ， 其 中 利用 所 有 输入 的 加 权 平 均 来 计算 去 模糊 化 后 
的 输出 。 由 此 ， 该 层 中 孤立 节点 的 输出 可 计算 为 
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_10 N, Zyi Ny Zu Zw Py Zr P, 10 


图 7.2 图 7.1 中 所 采用 的 隶属 函数 (经 IEEE 许可 ， 从 参考 文献 [44] 中 复制 ) 


3 3 
> Ow, X ulu’ )w, 
= i=l = i=l 


0" = = (7.32) 


DO Mun) 





4. 第 四 层 : 输出 层 
该 层 对 去 模糊 化 后 的 输出 进行 适当 缩放 。 该 层 中 节点 的 输入 、 输 出 关系 为 
0* = Ku = KO? (7. 33) 


7.4.2 利用 PSO 训练 模糊 神经 网 络 模型 


通过 训练 该 模糊 神经 网 络 模型 来 确定 系统 的 自由 参数 ， 即 隶属 函数 的 参数 、 
单个 输出 结果 和 输出 增益 。 然 而 在 传统 的 监督 模式 下 ， 对 于 一 个 给 定 输入 ， 由 
于 期 望 输出 无 法 定量 确定 而 导致 无 法 训练 模型 。 因 此 ， 对 于 这 种 无 监督 模式 的 
系统 ， 不 能 采用 常用 的 后 向 传播 训练 方式 ， 而 只 能 采用 一 种 合适 的 随机 全 局 优 
化 算法 。 目 前 ， 已 有 多 种 可 选 的 优化 算法 。 本 节 将 介绍 为 何 PSO 算法 能 够 适用 
于 这 种 情况 。PSO 算法 是 一 种 基于 乌 群 或 鱼 群 的 群体 行为 的 相对 较 新 的 算 
法 “” 。 模 糊 神 经 网 络 系统 的 训练 可 看 作 一 个 高 维 元 启发 式 优 化 问题 ， 目 标 是 
TEZE RAE xi, x), ce, ox, 的 基础 上 来 优化 适应 度 函 数 庆 (xz xs. cns x.) 

对 于 一 个 PSO 问题 ， 在 一 个 多 维 空间 中 创建 多 个 x, ，x,，…， x 的 可 能 解 
( 称 为 粒子 )， 并 在 每 次 迭代 中 估计 每 个 粒子 的 适应 性 。 在 本 书 所 考虑 的 情况 下 ， 
每 个 粒子 都 由 一 个 十 二 维 矢量 x = [x,，x;,，…，xis] 表示， 如 图 7.3 所 示 。 每 
个 粒子 i 是 由 表示 在 当前 时 间 步 长 下 的 位 置 (x,;) 和 速度 (vw) 矢量 来 表征 的 。 为 确 
定 适应 度 函 数 (f, ) 的 最 优 值 ， 在 每 个 时 间 步 长 中 调整 每 个 粒子 的 速度 (vw) 和 位 
置 (x,)。 每 个 时 间 步 长 中 更 新 后 的 速度 v,,, 是 以 下 三 个 主 元 素 的 一 个 函数 : 同一 
粒子 的 原 速 度 失 量 (vw,,)、 第 i 个 粒子 的 最 佳 位 置 ( 记 为 p) Sio gu E Cx) CRI 
为 认 知 分 量 ) 之 差 ， 以 及 第 i 个 粒子 拓扑 邻 域 中 任 一 粒子 的 最 佳 位 置 ( 记 为 p,) 与 
第 i 个 粒子 当前 位 置 (x,)( 称 为 “社会 "分量 ) 之 差 ” 。 对 后 两 个 分 量 都 随机 加 
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权 以 使 得 在 每 个 粒子 速度 更 新 时 能 产生 足够 振荡 ， 从 而 能 允许 每 个 粒子 在 问题 
空间 中 寻找 一 种 更 佳 的 模式 。 因 此 ， 在 第 d 维 中 ， 速 度 和 位 置 的 更 新 关系 为 
Vianew = Vana + Pi (Pia — Xia) + Pg (Pea 7 Xea) 
IF (vune > Vana) THEN v4,,, = Yama, ENDIF 
IF (vi < 74,5 THEN disney = -v,,,, ENDIF 


Xidnew = Xigold + Vidnew 


Vidold = Vidnew 
MXidold = © idnew ( T. 34 ) 


其 中 , o, Ale, HIT SIATBSBLBUER, Ario e; =c, x rand, ( O Hl o, 2 c, x 
rand, () , rand, ( ) ll rand() 为 L0，1] 范 围 内 的 两 个 随机 函数 ，c FIL e, 均 为 正常 
数 。 通 常 选择 c 和 es He, =cs =2。 传 统 的 PSO 模型 在 开始 阶段 能 够 快速 收敛 ， 
但 经 常会 在 确定 最 优 解 的 微调 阶段 有 问题 。 因 此 ， 本 算法 采用 一 种 改进 的 PSO 
算法 ， 即 一 种 在 迭代 初期 粗 调 ， 而 在 迭代 后 期 微调 的 快速 组 合 方法 ”。 相 应 的 
速度 更 新 规则 为 

Vas. = Was (Vigod) + pi(Pis 7 Xia) + ps (Pra 7 xu) (7.35) 
而 位 置 更 新 规则 保持 不 变 。w 称 为 惯性 权重 ,该 值 在 初始 时 较 大 ， 然 后 在 迭代 过 
程 中 逐步 减 小 ， 从 而 使 得 其 能 够 在 速度 更 新 中 先 粗 调 ， 然 后 逐步 微调 。 在 本 算 
法 中 ,采用 了 一 个 线性 自 适应 的 惯性 权重 ， 并 在 给 定 迭 代 中 ，wi,, 给 出 该 惯性 权 
重 值 。 继 续 执 行 该 迭代 过 程 ， 直 到 在 该 优化 过 程 中 产生 一 个 满意 解 。 该 解 是 根 
据 是 否 厌 值 小 于 给 定 最 大 允许 值 或 已 达到 最 大 迭代 次 数 来 评估 的 。 参 考 文献 
[40，41] 中 对 PSO 算法 进行 了 详细 阐述 。 





图 7.3 PSO 算法 中 每 个 十 二 维 粒 子 的 详细 配置 
(经 IEEE 许可 ， 从 参考 文献 [44 |] 中 复制 ) 


在 本 方法 中 ， 模 糊 神经 网 络 对 基于 EKF 的 SLAM 的 目标 是 尽 可 能 地 提高 估 
计 性 能 。 这 意味 着 在 车 辆 /机 器 人 运动 过 程 中 ， 应 尽 可 能 地 使 得 在 整个 观测 时 刻 
集中 新 息 序列 的 实际 协 方差 和 理想 协 方差 之 差 最 小 。 因 此 ， 可 根据 下 列 步骤 来 
构造 适应 度 函 数 : 计算 任意 观测 时 刻 时 AC,,, 和 矩阵 中 所 有 对 角 元 素 的 方 均 根 ; 在 
当前 迭代 过 程 中 ， 保 存 每 个 观测 时 刻 的 方 均 根 ; 在 一 次 完整 迭代 结束 时 ， 计 算 
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所 有 观测 时 刻 方 均 根 的 平均 值 。 从 数学 上 可 表示 为 
JC-nobs 


y | > [AC ion Gi) j 


nope =} EN 
fa = x 

式 中 NULLE 7 UR XE SNC Hn aes BR LIA Es Za C ; 

Jc sas AY na KARM, ERE ACuw 中 对 角 元 素 的 总 个 数 。 

对 于 一 种 有 实际 意义 的 自 适应 NFS， 每 次 迭代 结束 时 每 个 粒子 的 位 置 都 会 
受到 车 干 约束 条 件 的 限制 。 这 些 约 束 大 多 数 是 为 了 保证 隶属 函数 的 特定 形状 ( 通 
常 为 梯形 ， 某 些 特殊 情况 下 也 可 以 是 矩形 ) ， 并 保证 连续 隶属 函数 的 延长 线 重 
共 。 男 一 类 约束 是 应 以 单调 非 降 的 形式 来 选择 每 个 隶属 函数 的 控制 点 (从 左 到 右 
开始 ) 。 这 样 就 能 确保 在 NFS 输入 范畴 中 的 所 有 区 域 将 包含 至 少 一 个 隶属 函数 。 
上 述 约束 条 件 具体 表现 如 下 : 

IF(N, <N,) THEN N, =N, ENDIF 

IF (za <z,,) THEN z,, =z ENDIF 

IF (z,. <z,) THEN z,, =z, ENDIF 

IF (z,, <z,,) THEN z,, =z,, ENDIF 

IF (P, «P,) THEN P, =P, ENDIF 

IF (N, « Z4) THEN N, - Z,, ENDIF 

IF (Z,, « P,) THEN Z, =P, ENDIF (7.37) 

另 一 类 约束 是 表示 对 NFS 输出 层 的 缩放 只 是 幅 值 上 的 比例 缩放 。 因 此 ， 该 
约束 不 能 用 于 改变 输出 极 性 ， 这 也 意味 着 天 不 能 为 负 值 。 


7.4.3 性 能 评估 


为 了 评估 本 书 系统 的 性 能 ， 在 此 考虑 参考 文献 [42] 中 所 述 的 不 同 环境 。 实 
际 上 ， 参 考 文献 [42] 所 提供 的 软件 包 可 作为 学 习 和 分 析 现 有 基于 EKF 和 粒子 滤 
波 的 SLAM 算法 的 一 个 良好 平台 。 在 该 平台 上 ， 研 究 人 员 可 开发 各 自 的 算法 并 
进行 性 能 比较 。 在 此 ， 提 供 了 几 种 基准 环境 ， 并 在 这 些 仿真 环境 下 结合 所 关联 
的 给 定 车 辆 /移动 机 器 人 运动 模型 来 测试 本 书 所 提出 的 算法 。 通 常 以 一 种 方式 来 
定义 环境 ， 即 假设 通过 某 些 路 标点 来 进行 车 辆 /机 器 人 导航 ， 并 在 此 过 程 中 ,能 
够 获得 包含 一 些 特征 /路 标点 的 环境 地 图 。 在 所 提 的 方法 中 ， 考 虑 了 参考 文献 
[42] 中 所 指定 的 三 种 环境 。 每 种 环境 下 ， 都 认为 理想 机 器 人 在 由 十 七 个 导航 点 
确定 的 路 径 上 运动 。 然 而 ， 每 种 环境 中 所 包含 的 路 标点 个 数 会 变化 以 增加 复杂 
度 ， 在 此 所 考虑 的 三 种 环境 中 分 别 包 含 35、135 和 497 个 路 标 。 指 定 输入 控制 量 
中 的 不 确定 性 为 oa, =0.3m/s，o,=3°。 经 过 八 个 连续 的 预测 步骤 之 后 ， 执 行 一 





(7.36) 
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次 观测 和 相关 联 的 更 新 ， 这 与 参考 文献 [42 ] 中 基于 EKF 的 算法 相同 。 在 此 ， 沿 
用 基于 EKF 的 SLAM 算法 中 的 常用 概念 ， 并 不 是 在 每 次 预测 之 后 都 执行 一 次 观 
测 和 更 新 ， 而 是 计算 几 个 连续 预测 之 后 ， 由 一 次 观测 和 更 新 来 修正 。 这 样 有 助 
于 减少 SLAM 算法 的 计算 量 。 在 算法 7.1 rp, 用 Time_ for | Observation 标志 位 
来 标识 ， 即 每 八 次 连续 迭代 之 后 ， 下 一 次 迭代 设 为 真 。 

在 此 ， 对 本 书 所 提出 的 系统 与 传统 的 Q、R 矩阵 在 整个 实验 过 程 中 保持 不 变 
的 基于 EKF 的 SLAM 系统 进行 了 性 能 比较 。 在 本 书 算法 中 ,也 是 以 相同 的 Q、 
R 矩阵 开始 执行 ， 但 不 同 的 是 ， 不 断 自 适应 矩阵。 根据 参考 文献 [42] 所 提供 
的 数据 ， 当 已 知 传感器 统计 值 为 o, =0.1m，o, =1° 时 ， 基 于 EKF 的 SLAM 算法 
效果 很 好 。 首 先 ， 考 虑 若 传感器 统计 值 被 误 认 为 o, =0.2m, e, =0.1° 时 的 情况 。 
后 文 介绍 的 图 中 ， 细 实 线 表示 机 器 人 实际 经 过 的 路 径 ， 而 点 线 表 示 根 据 每 次 采 
样 时 刻 或 迭代 过 程 中 机 器 人 位 姿 的 估计 状态 由 SLAM 算法 估计 的 路 径 ， 星 形 线 
(x ) 表 示 环 境 中 静态 路 标的 实际 位 置 ， 十 字形 线 ( ) 表示 测 试 结束 后 这 些 路 标 
的 估计 位 置 。 显 然 ， 如 果 机 器 人 的 估计 路 径 和 实际 路 径 尽 可 能 地 匹配 ， 且 路 标 
的 估计 位 置 与 其 实际 位 置 尽 可 能 地 一 致 ， 则 系统 性 能 将 更 优 。 图 7.4 给 出 了 三 
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图 7.4 示例 工 中 (c, =2.0m, o, =0.1°) " 传统 的 基于 EKF 的 SLAM 算法 的 性 能 ， 
其 中 环境 中 具有 35( 见 图 7.4a) 、135( 见 图 7.4b) 和 497( 见 图 7.4c) 个 特征 /路 标 
(经 IEEE 许可 ， 从 参考 文献 [44] 中 复制 ) 

D ”此 处 原 书 有 误 ，o, 应 为 ru。 一 一 译 者 注 
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图 7.4 WØ IPC, 22.0m, o, =0.1?) 传 统 的 基于 EKF 的 SLAM 算法 的 性 能 ， 
其 中 环境 中 具有 35( 见 图 7.4a) 、135( 见 图 7.4b) 和 497( 见 图 7.4c) 个 特征 /路 标 
(经 IEEE 许可 ， 从 参考 文献 [44] 中 复制 )( 续 ) 
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种 不 同 环境 情况 下 传统 的 基于 EKF 的 SLAM 算法 的 性 能 。 由 图 可 知 ， 若 环境 中 
的 路 标 较 少 ， 系 统 性 能 还 是 可 接受 的 。 然 而 当 路 标 变 得 更 加 密集 时 ， 系 统 性 能 
就 会 变 得 相当 差 ， 并 且 随 着 EKF 的 估计 位 置 与 机 右 人 实际 位 置 相距 其 远 ， 则 不 
同 采样 时 刻 机 器 人 的 估计 位 置 和 所 构建 的 地 图 也 将 急剧 变 差 。 图 7.5 给 出 了 相 
同 环境 下 采用 结合 模糊 神经 网 络 的 基于 EKF 的 SLAM 算法 的 情况 。 由 图 可 知 ， 
模糊 神经 网 络 能 够 在 很 大 程度 上 提高 系统 性 能 ， 同 时 在 路 标 个 数 变 化 的 三 种 不 
同 环境 下 ， 机 器 人 的 状态 估计 以 及 所 构建 的 地 图 也 都 很 稳定 。 在 所 有 三 种 环境 
中 ， 机 器 人 的 估计 位 置 与 其 实际 位 置 非常 接近 ， 静 态 路 标的 估计 位 置 也 与 其 实 
际 位 置 非常 匹配 。 

进一步 对 传感器 统计 值 被 误 认 为 相反 方向 ( 即 o, =0.01m, o, =3. Odeg) #3) 
另 一 种 情况 进行 测试 。 在 此 ， 对 同样 的 环境 集 采 用 了 相同 的 算法 集 。 图 7.6 给 
出 了 传统 基于 EKF 的 SLAM 算法 的 性 能 ， 图 7.7 给 出 了 结合 模糊 神经 网 络 的 基 
于 EKF 的 SLAM 算法 的 性 能 。 在 这 些 示例 情况 下 ， 基 于 EKF 的 SLAM 算法 表现 
出 不 同 的 性 能 趋势 。 

















a) 


图 7.5 示例 I 中 (o, 22.0m, o, =0.1°) 结合 模糊 神经 网 络 的 基于 EKF 
的 SLAM 算法 的 性 能 ， 其 中 环境 中 具有 35( 见 图 7.5a) 、135( 见 图 7.5b) 和 
497( 见 图 7.5c) 个 特征 /路 标 (经 IEEE 许可 ， 从 参考 文献 [44] 中 复制 ) 

”此 处 原 书 有 误 ，o, 应 为 ro。 译 者 注 
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c) 
图 7.5 ABI (Co, 22.0m, o, =0.1?) 结 合 模糊 神经 网 络 的 基于 EKF 的 


SLAM 算法 的 性 能 ， 其 中 环境 中 具有 35( 见 图 7.5a) 、135( 见 图 7.5b) 和 497 
( 见 图 7.5c) 个 特征 /路 标 (经 IEEE 许可 ， 从 参考 文献 [44] 中 复制 )( 续 ) 
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图 7.6 RAPo, =0.01m, o, 23.0) V 传统 的 基于 EKF SLAM 性 能 ， 其 中 环境 中 具有 
35( 见 图 7.6a) 135( 见 图 7.6b) 和 497( 见 图 7.6c) 个 特征 /路 标 (经 IEEE 许可 ， 从 参考 文 


献 [44] 中 复制 ) 
(D ARBAR, o, 应 为 go。 一 一 译 者 注 
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c) 


图 7.6 WAP, 20.01m, o, =3.0?) 传 统 的 基于 EKF SLAM 性 能 ， 其 中 环境 中 具有 
35( 见 图 7.6a) 135( 见 图 7.6b) 和 497( 见 图 7.6c) 个 特征 /路 标 (经 IEEE 许可 ， 从 参考 文 
献 [44] 中 复制 )( 续 ) 

由 图 可 看 出 ， 环 境 中 包含 少量 路 标 时 估计 性 能 最 差 。 然 而 随 着 路 标 个 数 增加 ， 
估计 得 也 越 来 越 准确 。 对 于 包含 497 个 路 标的 情况 ， 基 于 EKF 的 SLAM 算法 性 能 
已 经 相当 满意 了 。 男 一 方面 ， 结 合 模 糊 神 经 网 络 的 基于 EKF 的 SLAM 算法 对 于 每 
种 环境 都 表现 非常 稳定 ， 机 器 人 位 姿 和 特征 位 置 都 估计 得 相当 准确 。 如 图 7.5 和 图 
7.7 所 示 ， 结 合 模糊 神经 网 络 的 基于 EKF 的 SLAM 算法 的 每 个 结果 都 是 在 一 次 运 
行 中 得 到 的 。 对 于 两 种 示例 情况 下 的 6 个 特定 场景 ， 共 进行 了 10 次 独立 的 运行 。 
实验 发 现 ， 对 于 每 种 特定 情况 ，10 次 独立 运行 的 结果 都 非常 接近 。 同 时 ， 这 些 实 
验 也 进一步 证 明了 在 传感器 统计 值 有 误差 的 情况 下 ,结合 模糊 神经 网 络 的 基于 
EKF 的 SLAM 算法 可 极 大 地 提高 传统 算法 的 性 能 。 在 上 述 情况 下 ， 传 统 EKF 算法 
的 性 能 相当 不 可 靠 。 然 而 模糊 神经 网 络 能 够 帮助 EKF 算法 保持 一 个 稳定 的 性 能 ， 
且 已 表明 在 传感器 统计 值 有 误差 的 几 种 环境 下 ， 该 性 能 具有 足够 的 鲁 棒 性 。 

对 于 结合 模糊 神经 网 络 的 基于 EKF 的 SLAM 算法 ,在 上 述 讨 论 的 每 种 情况 
下 ， 都 能 根据 一 个 给 定 的 环境 下 机 器 人 所 采集 的 数据 来 离线 训练 模糊 神经 网 络 
系统 。 在 实验 中 ， 也 实现 了 包含 135 个 路 标的 每 种 示例 环境 下 的 训练 过 程 。 一 
且 模 糊 神经 网 络 系统 训练 完成 (在 一 个 给 定 路 标 配 置 的 基础 上 )， 并 确定 NFS 的 
自由 参数 之 后 ， 就 针对 如 上 所 述 的 几 种 路 标 配 置 ( 即 环 境 中 包含 35 135 和 497 
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个 路 标 ) ， 利 用 训练 后 的 基于 NFS 的 EKF 算法 来 实现 机 器 人 导航 。 表 7.1 详细 
列 出 了 训练 NFS 时 所 用 PSO 算法 的 参数 。 在 此 ， 用 于 学 习 NFS 中 隶属 函数 控制 
点 ( 即 [x,xs…%s]) 的 每 个 粒子 的 维 数 均 初始 化 为 [ -1,，1] 范 围 内 的 粒子 位 置 ， 
这 是 在 与 NFS 共同 工作 的 归 一 化 过 程 中 构成 的 。 与 NFS 中 第 三 层 关联 的 预期 权 
重 ( 由 PSO 算法 的 维 数 x。、xi。 和 x 表示 ) 均 初始 化 为 [ -2，2] 范 围 内 的 位 置 ， 
与 NFS 中 第 四 层 关联 的 预期 增益 K( 由 PSO 算法 的 维 数 x, 表 示 ) 初 始 化 为 [0，2] 
范围 内 的 位 置 ， 这 是 因为 在 此 假设 K 为 非 负 值 。PSO 算法 的 终止 条 件 每 次 都 设 
为 最 大 迭代 次 数 20。 初 始 传感器 信息 o, =0.2m, e, 20.1? 的 示例 情况 下 ， 训练 
过 程 完成 时 NFS 的 学 习 参 数 为 
[x,x,":x,,] =[-0.2008 -0.0626 -0.0626 -0.0626 0.0820 
0.5961 0.3224 0.4002 -0.0086 1.5801 
- 0.9729 0.0011] 
对 于 初始 传感器 信息 o, =0.01m, o, =3.0° 的 示例 情况 ，NFS 的 学 习 参 数 为 
[x,x,:x,] = [ - 0.4570 0.5242 0.4805 0.9741 0.9741 0.9741 
— 0.4290 0.2413 — 0. 0024 — 0.8762 1.3561 
0. 2907 | 
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a) 

图 7.7 WAP, 20.01m, o, =3.0°) 结合 模糊 神经 网 络 的 基于 EKF 的 
SLAM 算法 的 性 能 ， 其 中 环境 中 具有 35( 见 图 7.7a) 、135( 见 图 7.7b) 和 
497( 见 图 7.7c) 个 特征 /路 标 (经 IEEE 许可 ， 从 参考 文献 [44] 中 复制 ) 

QD 此 处 原 书 有 误 ，cv 应 为 nu. 一 一 译 者 注 
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c) 


图 7.7 ABI Co, =0.01m, o, =3.0?) 结 合 模糊 神经 网 络 的 基于 EKF 的 
SLAM 算法 的 性 能 ， 其 中 环境 中 具有 35( 见 图 7.7a) 、135( 见 图 7.7b) 和 
497( 见 图 7.7c) 个 特征 /路 标 (经 IEEE 许可 ， 从 参考 文献 [44] 中 复制 )( 续 ) 
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由 上 可 看 出 ， 在 每 种 情况 下 ， 这 些 学 习 参 数 都 满足 式 (7.37) 中 的 约束 条 件 。 
表 7.1 所 采用 的 PSO 算法 参数 















































SI 序号 参数 描述 示例 工 的 参数 值 示例 下 的 参数 值 
1 粒子 数 (NN) 40 40 
2 维 数 (D) 12 12 
3 初始 惯性 权重 ( Wi,iia) 0.9 0.9 
4 惯性 权重 ( AW) 2.5e-4 2.5e-4 
5 SE PAB) BMA (xa. xo. cs, xg) [ -1, 1] [.-1, 1] 
6 权重 因子 的 初始 范围 (x。， Xio» £g) [ -2, 2] [ -2. 2] 
7 增益 的 初始 范围 (x1, ) [0, 2] [0, 2] 
隶属 函数 的 最 大 允许 速度 (vs Coma 
8 0.3 0.1 
Ugmax ) 
权重 因子 的 最 大 允许 速度 (vowss， Viomar， 
9 1.0 0.5 
Dilmar) 
IER 
10 38 ag B Je fe VE ERE (Viamax ) 1.0 0.5 














7.5 KA DE 优化 算法 训练 模糊 监督 器 


7.4 节 阐述 了 如 何 利 用 PSO 算法 训练 一 个 模糊 /模糊 神经 网 络 监督 器 来 成 功 
地 监督 基于 EKF 的 SLAM 系统 。 从 逻辑 上 来 说 ,该 思想 可 扩展 到 其 他 进化 算法 
来 实现 类 似 的 基于 模糊 /模糊 神经 网 络 的 监督 。 因 此 ， 在 此 实现 了 一 种 采用 差分 
进化 (DE) 优 化 算法 的 类 似 的 模糊 监督 器 ， 差 分 进化 算法 是 男 一 种 解决 该 类 问题 
的 常用 进化 算法 “” 。 类 似 于 其 他 常用 的 全 局 优化 方法 ， 差 分 进化 算法 也 是 在 多 
维 搜索 空间 中 同时 创建 几 个 候选 解 ， 其 中 每 个 解 都 包含 一 个 该 优化 问题 的 可 能 
ERE, 并且， 每 个 解 都 由 表示 特定 解 进化 为 最 优 解 的 适应 性 程度 的 适应 度 函 
数 来 单独 评估 。 和 迭代 执行 该 过 程 ， 其 中 从 之 前 所 产生 的 可 能 解 中 创建 新 矢量 ( 即 
可 能 的 候选 解 ) ， 来 产生 可 由 适应 度 函 数值 定量 估计 的 越 来 越 优 的 解 。 根 据 上 次 
产生 的 候选 矢量 ， 可 采用 几 种 数学 策略 来 创建 本 次 新 的 候选 矢量 。 在 每 次 产生 
结束 时 ， 候 选 解 能 够 给 出 可 能 产生 最 优 解 的 最 佳 适应 度 函 数值 (通常 为 最 小 值 )。 
继续 该 迭代 过 程 ， 直 到 某 次 产生 最 优 解 向 量 的 最 佳 适 应 度 函 数值 低 于 该 优化 过 
程 所 允许 的 最 大 适应 度 函 数值 ， 或 达到 最 大 进化 代数 。 

此 处 ,考虑 在 D AE x 的 基础 上 ， 用 于 最 小 化 代价 函数 A(x) 的 基本 差分 进化 
算法 中 ，(x,，x,，…，xb) 的 候选 解 的 NP 个 数 创建 在 一 个 D 维 空间 中 ， 并 在 每 
代 G 中 评估 每 个 解 的 适应 性 。 随 机 产生 初始 种 群 ， 目 标 是 使 得 产生 的 矢量 应 尽 
可 能 地 涵盖 整个 搜索 空间 。 通 过 在 第 三 个 矢量 中 增加 两 个 种 群 矢量 之 间 的 权重 
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之 差 (这 三 个 矢量 都 属于 第 G 代 ) 来 创建 第 G+1 代 中 所 有 第 i 个 矢量 ,这 可 由 下 
gm, 
figu = We + P(N — 5 c) (7.38) 
式 中 i=l, 2, =, NP; 
nm，m，mpe[l，NP] 且 均 相 互 不 同 ; 
一 一 一 个 恒定 的 权重 因子 ， 且 通常 Fe10, 2]， 该 因子 可 影响 差 值 (x,,。 
-xc) 的 幅 值 。 

为 增加 新 产生 矢量 的 多 样 性 ， 在 此 引入 交叉 方法 。 交 叉 操 作 可 从 新 产生 的 
扰动 矢量 vv ca MERE x; sc 中 产生 一 个 新 矢量 u,,,。 在 基本 差分 进化 算法 中 ， 
所 产生 的 该 新 矢量 为 x 
Uca = (Uiiers Uaicrs sUpi,cr) 

Us cut 对 于 j=(n+1)pe(n +L), 
L. 对 于 “其 他 Je [1,D] 
一 个 随机 选取 的 整数 ，ne [1,，D]， 并且 决 定 了 交叉 操作 的 起 始 序 


(7. 39 ) 





式 中 n 
号 。 

在 这 种 基本 的 差分 进化 算法 中 ， 交 叉 长 度 或 持续 时 间 是 根据 所 选择 的 交叉 
概率 (CRe [0, 1]) 在 [1,，D] 中 选取 的 一 个 整数 。 对 每 个 u, ce, 都 重新 选择 这 些 
n All LB. 

如 果 新 矢量 u, ,| 可 产生 一 个 较 小 的 适应 度 函 数值 ， 则 该 矢量 就 是 第 G+1 
代 的 新 x ,,, ， 否 则 保持 Xici = Xe 


7.5.1 性 能 评估 


通过 创建 一 个 仿真 环境 ， 并 采用 参考 文献 [42 1 中 所 提供 的 软件 包 ， 按照 前 
面 示例 中 的 设置 ， 对 基于 差分 进化 优化 算法 的 模糊 监督 器 求解 SLAM 问题 的 性 
能 进行 测试 。 在 新 的 示例 中 ， 考 虑 一 个 不 同 环境 下 两 组 传感器 统计 值 信息 有 误 
的 情况 : c, =0.01m, o,=10.0°°; o, -0.01m, o, =15.0。。 针 对 这 些 情 况 ， 图 
7.8 给 出 了 传统 的 基于 EKF 的 SLAM 算法 ”的 性 能 结果 。 由 图 可 知 ， 所 估计 的 
机 器 人 路 径 与 理想 路 径 存 在 很 大 偏差 ， 并 且 所 估计 的 许多 路 标 位 置 也 与 其 实际 
位 置 相差 其 远 。 然 而 若 对 两 种 示例 采用 基于 差分 进化 优化 的 模糊 监督 器 的 系统 ， 
模糊 监督 都 能 大 幅 提 高 性 能 ， 如 图 7.9 所 示 。 对 于 模糊 监督 算法 ， 估 计 的 机 器 
人 路 径 与 理想 路 径 偏 差 较 小 。 在 该 方法 中 ， 以 D =11 的 差分 进化 并 结合 二 项 式 
交叉 来 自动 学 习 模 糊 监 督 器 的 自由 参数 。 所 采用 的 差分 进化 算法 是 一 种 称 为 





O KPPK, BABAR., 均 应 为 oo。 一 一 译 者 注 


图 7.8 
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b) 


在 传感器 统计 信息 有 误 的 情况 下 ， 传 统 的 基于 EKF 的 SLAM 算法 的 性 能 


a)o, =0.01m, ao, =10.0° b) o =0.01lm, oa, =15.0° 
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图 7.9 在 传感器 统计 信息 有 误 的 情况 下 ， 结 合 模糊 监督 的 
传统 的 基于 EKF 的 SLAM 算法 的 性 能 
a)g, -0.01m, o, =10.0° b) g, =0.01m, o, =15.0° 
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“DE/rand/1" 的 常用 形式 “* |。 然而 这 种 算法 与 原始 的 “DE/rand/1" 方 法 有 很 大 
不 同 ， 由 于 是 在 对 包含 种 群 的 数组 重新 排列 后 随机 选取 矢量 的 ， 因 此 一 个 给 定 
矢量 不 能 在 含有 扰动 的 同一 项 中 重复 选择 两 次 。 另 外 ， 还 可 看 出 ， 每 种 示例 
下 ， 路 标的 估计 位 置 与 其 实际 位 置 都 要 比 采 用 传统 的 基于 EKF 的 SLAM 算法 更 
加 接近 。 

利用 差分 进化 算法 训练 的 模糊 监督 器 ， 并 选择 控制 参数 为 NP =20, F=0.1 
和 CR =0.5， 所 得 到 的 结果 如 图 7.9 所 示 。 与 大 多 数 其 他 的 随机 全 局 优化 方法 相 
同 ， 差 分 进化 策略 的 性 能 会 根据 选择 不 同 的 自由 参数 而 发 生 很 大 变化 。 因 此 选 
择 合适 的 或 准确 拟 合 这 些 自由 参数 非常 关键 。 根 据 参考 文献 [46] 中 提出 的 一 般 
准则 ， 对 于 许多 应 用 ， 认 为 选择 NP -10D, Fe[0.5, 1], CRe[0, 1] 且 远 小 
于 1 是 比较 理想 的 。 在 这 些 参数 中 ,FF 是 最 关键 的 控制 参数 ， 其 次 是 NP 和 CR。 
因此 ， 为 达到 差分 进化 的 最 佳 性 能 ， 应 采用 各 种 不 同 的 控制 参数 值 来 进行 仿真 ， 
并 观察 相应 的 性 能 。 对 于 传感器 统计 值 为 o, 20.01m, o, =10.0° 的 示例 情况 。 
首先 ，NP 和 CR 分 别 保持 为 20 和 0.5, FF 在 0~1 变化 ， 并 在 每 种 情况 下 对 模糊 
监督 器 进行 训练 。 尽 管 根据 一 般 准 则 ，NP 应 选择 为 10 x 11 =110, 但 这 仍 将 会 
大 大 增加 训练 过 程 的 计算 量 。 因 此 ， 考 虑 到 将 计算 量 维持 在 较 小 的 一 个 合理 范 
围 内 ， 优 化 过 程 的 NP 值 选 为 20。 在 此 ， 随 着 下 值 的 变化 ， 表 明 若 采用 较 小 的 下 
值 ， 则 整个 系统 的 性 能 会 越 来 越 好 。 最 后 得 出 在 f=0.1 时 可 达到 最 佳 性 能 ， 若 
值 更 小 ， 性 能 会 有 所 下 降 ， 而 值 较 大 时 ， 性 能 下 降 明 显 。 图 7. 10 给 出 了 下 
20.05, F=0.1 fü F 20.5 时 ， 具 有 训练 模糊 监督 器 的 系统 的 各 自 性 能 。 图 


7.11 给 出 也 为 五 种 典型 值 下 ， 随 着 机 器 人 增 量 运动 ， 每 个 采样 时 刻 估计 x 的 方 
均 根 误差 。 由 此 可 得 出 =0.1 时 训练 过 程 可 产生 最 佳 的 实验 结果 。 

根据 该 值 ， 可 继续 确定 NP 和 CR 的 最 佳 值 。 保持 =0.1 和 CR 20.5, 
变化 NP 值 ， 目 的 是 在 NP 尽量 小 的 情况 下 达到 一 个 满意 性 能 ， 从 而 使 得 计算 量 
最 小 。 图 7. 12 给 出 了 NP 215, 20 和 25 时 机 器 人 增 量 运动 下 ， 每 个 采样 时 刻 所 


估计 的 x 的 方 均 根 误差 。 由 图 可 知 ，NP = 20 时 可 获得 最 佳 性 能 ， 如 果 增 大 或 减 
小 NP 值 都 会 使 得 性 能 下 降 ， 因 此 在 训练 过 程 中 选取 NP 220, Fe POR, REF F = 
0.1 和 NP=20， 变 化 CR 值 。 在 此 发 现 CR 的 变化 并 不 是 改变 训练 性 能 的 关键 因 


素 。 图 7. 13 给 出 了 CR =0.4、0.5 和 0.6 时 zx 估计 的 方 均 根 误差 。 尽 管 对 于 其 他 
CR 值 性 能 都 基本 上 非常 相近 ， 但 还 是 可 看 出 CR =0.5 时 能 够 达到 最 佳 性 能 。 因 
此 ， 可 得 出 模糊 监督 器 训练 过 程 中 差分 进化 算法 的 最 佳 控 制 参数 应 是 NP = 20、 
玉 =0.1 和 CR=0.5。 对 于 每 种 传感器 统计 的 示例 情况 下 ， 采 用 这 些 参数 对 模糊 
监督 器 进行 训练 ， 即 o, =0.01m, o, =10.0°; o =0.0lm, o, =15.0° 。 图 7.9 
给 出 了 这 些 示 例 的 性 能 结果 。 
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b) 


图 7.10 以 NP 220, CR 20.5 进行 基于 差分 进化 算法 训练 
的 模糊 监督 的 基于 EKF 的 SLAM 算法 性 能 
a)F=0.05 b)F=0.1 
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E] 7.10 以 NP 220, CR 20.5 进行 基于 差分 进化 算法 训练 
的 模糊 监督 的 基于 EKF 的 SLAM 算法 性 能 ( 续 ) 
c)F =0.5 




















1500 2000 2500 3000 
采样 时 刻 
图 7.11 以 NP=20、CR =0.5 进行 基于 差分 进化 算法 训练 的 
模糊 监督 的 基于 EKF 的 SLAM 算法 性 能 
a—F=0.05 b—F=0.08 c—F=0.1 d—F=0.15 e—F=0.5 
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方 均 根 误差 
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采样 时 刻 
图 7.12 WF =0.1, CR=0.5 进行 基于 差分 进化 算法 训练 的 


模糊 监督 的 基于 EKF 的 SLAM 算法 性 能 
a—NP-15 b—NP-20 c—NP=25 
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图 7.13 VA Fz0.1, NP =20 进行 基于 差分 进化 算法 训练 的 
模糊 监督 的 基于 EKF 的 SLAM 算法 性 能 
a—CR=0.4 b—CR=0.5 c—CR=0.6 
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图 7.14 以 惯性 权重 斜率 为 2.0e -4a) , 2.5e - 4b) , 5.0e - 4c) , 4e - 2d) fll 5e - 2e) 
进行 基于 PSO 算法 训练 的 模糊 监督 的 基于 EKF 的 SLAM 算法 性 能 





方 均 根 误差 
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采样 时 刻 

图 7.15 模糊 监督 器 分 别 由 差分 进化 算法 (a) 和 PSO 算法 训练 (b) 的 模糊 监督 的 

基于 EKF 的 SLAM 算法 性 能 评估 比较 
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接 下 来 ， 对 由 差分 进化 算法 调节 的 模糊 监督 器 和 由 PSO 算法 调节 的 模糊 
监督 器 进行 性 能 比较 。 在 此 是 针对 传感器 统计 为 m, =0.01m, o, = 15. 0°Hf 
的 示例 进行 性 能 比较 ， 其 中 PSO 算法 中 采用 了 如 式 (7.35) 所 示 的 线性 递减 
惯性 权重 。 为 使 得 基于 差分 进化 算法 和 基于 PSO 算法 参数 调节 的 算法 进行 
统一 比较 ,在 此 考虑 了 以 下 因素 : 每 个 算法 的 候选 解 或 粒子 数 相同 (如 20 ) ， 
每 次 优化 算法 运行 的 最 大 迭代 次 数 相 同 ( 在 此 取 10); 每 个 优化 算法 中 相应 
的 初始 种 群 维 数 的 初始 范围 相同 。 通 常情 况 下 ， 初始 惯性 权重 Wu 0.9, 
惯性 权重 的 范围 为 2.5e -4 的 PSO 算法 对 于 基准 优化 函数 较为 理想 。 在 本 
示例 中 ， 采 用 了 Wiru =0.9 以 及 惯性 权重 缓慢 减 小 和 急剧 减 小 的 PS0。 图 
7.14 给 出 了 当 基 于 PSO 的 训练 过 程 中 惯性 权重 斜率 分 别 为 2.0e -4、2.5e 
-4、5.0e-4、4e-2 和 5S5e-2 时 PSO 算 法 的 相应 性 能 ， 这 是 通过 机 器 人 增 


量 运 动 的 每 个 采样 时 刻 估计 zx 的 方 均 根 误差 来 反映 的 。 由 此 可 看 出 ， 最 佳 性 
能 是 在 值 为 2.5e -4 时 获得 的 。 图 7.15 给 出 了 本 例 条 件 下 ， 基 于 自 适 应 
EKF 的 SLAM 算法 中 最 佳 PSO 调节 和 最 佳 DE 调节 模糊 监督 器 的 估计 性 能 比 
较 。 可 看 到 DE 调节 算法 的 性 能 ,在 大 多 数 采 样 时 刻 ， 具 有 较 小 的 方 均 根 估 
计 误 差 。 该 过 程 表 明了 对 于 基于 EKF 的 SLAM 问题 ， 采 用 DE 调节 的 模糊 监 
督 器 具有 很 大 作用 。 然 而 还 应 注意 到 ， 该 性 能 会 根据 所 选 环 境 和 传感器 统计 
值 的 变化 而 变化 。 


7.6 本 章 小 结 


本 章 讨 论 了 SLAM 在 移动 机 器 人 导航 中 的 重要 性 ， 并 首先 详细 介绍 了 基于 
EKF 的 SLAM 算法 。 接 下 来 ， 认 为 在 传感器 统计 值 的 先 验 知识 有 误 时 ， 系 统 性 
能 会 下 降 ， 并 表明 了 为 何 利 用 模糊 /模糊 神经 网 络 辅助 或 监督 能 够 显著 提高 算法 
性 能 。 通 常 ， 在 关联 统计 模型 完全 已 知 时 ， 认 为 EKF 对 SLAM 算法 而 言 是 一 种 
不 错 的 选择 。 然 而 当 统 计 信息 不 够 准确 时 ， 性 能 就 可 能 变 得 根本 无 法 预测 或 下 
降 。 基 于 模糊 /模糊 神经 网 络 监督 器 的 系统 能 够 以 错误 的 统计 值 开 始 ， 在 线 调整 
和 矩阵 RR， 使 得 新 息 序 列 理论 值 与 实际 值 之 间 的 误差 最 小 。 采 用 一 个 基于 进化 优化 
算法 的 训练 过 程 ， 可 自动 学 习 模 糊 神经 网 络 系统 的 自由 参数 。 本 章 表 明 两 种 常 
用 的 进化 优化 方法 ( 即 PSO 和 DE) 如 何 能 够 成 功 地 用 于 实现 该 目标 。 针 对 几 种 具 
有 不 同 传感器 统计 值 错误 信息 的 标准 环境 进行 了 性 能 评估 。 在 许多 情况 下 ， 传 
统 的 基于 EKF 的 SLAM 算法 表现 出 性 能 不 可 靠 并 急剧 下 降 ， 而 结合 模糊 /模糊 神 
经 网 络 可 显著 提高 EKF 的 性 能 ， 并 在 各 种 示例 的 每 种 情况 下 都 能 具有 和 鲁 棒 且 准 
确 的 性 能 。 

致谢 ”该 研究 工作 得 到 了 日 本 国外 研究 人 员 博 士 后 研究 基金 JSPS 的 部 分 资 


第 7 章 移动 机 器 人 的 SLAM 191 





助 。 另 外 还 得 到 了 印度 技术 教育 委员 会 RPS 项 目的 部 分 资助 (资助 号 : 8023/ 
BOR/RPS - 89/2006 -07) 。 
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摘要 : 本 章 是 对 第 7 章 内 容 的 扩展 和 补充 ， 讨 论 了 如 何在 室内 环境 
实现 第 7 章 所 介绍 的 移动 机 器 人 SLAM。 所 开发 的 机 器 人 系统 具有 一 个 包 
摄像 头 的 视觉 系统 ， 可 用 于 图 像 特征 识别 与 跟踪 。 


下 真正 
4 ^ 


8.1 简介 


如 第 7 章 所 述 ， 基 于 EKF 的 方法 已 被 普遍 认为 是 解决 移动 机 器 人 SLAM 
问题 的 最 适合 的 方法 ""”。EKF 在 解决 SLAM 问题 上 最 基本 的 能 力 在 于 迭代 评 
佑 的 方法 ， 并 由 此 建立 一 个 通过 某 些 导 航 点 直接 对 机 器 人 导航 的 环境 增 广 地 
图 。 在 此 假设 机 器 人 位 姿 的 初始 位 置 和 所 要 构建 的 地 图 均 未 知 ， 并 将 地 图 看 
作 一 种 增 广 估计 状态 来 逐步 构建 地 图 ， 这 时 地 图 中 只 包含 一 些 在 环境 中 采集 
的 特征 或 路 标的 位 置 ， 以 及 机 器 人 的 位 姿 。 这 些 状 态 的 估计 都 与 其 中 的 某 些 
不 确定 性 相关 联 ， 并 以 一 种 误差 协 方差 矩阵 的 形式 保存 。 在 第 7 章 已 详细 讨论 
了 基于 EKF 的 SLAM 算法 。 本 章 将 讨论 如 何 基 于 视觉 感知 来 实现 移动 机 器 人 
的 SLAM, 

另外 ， 还 要 注意 到 在 实际 环境 中 真正 实现 SLAM 算法 来 构建 一 个 有 意义 的 
地 图 是 一 项 艰巨 的 任务 。 该 系统 的 准确 性 很 大 程度 上 取决 于 所 用 的 传感器 。 
正如 我 们 所 知 ， 里 程 计 会 受 车 轮 打 滑 的 影响 ， 声 纳 传 感 器 的 分 辩 率 较 低 而 不 
是 一 个 非常 精确 的 系统 ， 并 受 环境 干扰 的 影响 ,红外 传感器 只 能 用 于 短 距 离 
的 测量 ， 而 激光 测 距 仪 成 本 较 高 ， 而 且 由 于 更 新 速率 较 低 而 导致 响应 较 慢 ， 
GPS 的 性 能 会 由 于 卫星 信号 的 遮挡 而 受到 影响 ， 造 成 准确 性 和 更 新 率 降低 。 因 
此 近年 来 ,摄像 尖 和 电脑 作为 一 种 有 吸引 力 的 、 可 行 的 实时 方案 来 构建 这 种 
机 大 人 定位 系统 ” 。 同 时 ,成 本 相对 低廉 ， 并 能 够 对 机 器 人 平台 的 导航 环境 
提供 灵活 解释 。 然 而 直到 目前 ， 有 关 基 于 视觉 感知 的 SLAM 算法 的 研究 还 很 
少 ， 主 要 原因 是 开发 该 系统 以 及 在 实际 中 达到 较 高 的 准确 性 是 一 项 艰巨 的 任 
务 。 

















加 “本章 基于 Avishek Chatterjee, Olive Ray, Amitava Chatterjee 和 Anjan Rakshit F 2011 年 7 月 在 Expert 
Systems with Applications 期 刊 上 第 38 卷 第 7 期 8266 ~ 8274 页 上 发 表 的 “Development of a Real - Life 
EKF based SLAM System for Mobile Robots employing Vision Sensing” 论文 。 
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本 章 将 对 用 于 室内 环境 中 地 图 构建 的 SLAM 算法 详细 介绍 其 具体 实现 过 
程 ”， 在 此 是 针对 一 种 已 在 之 前 章节 中 介绍 过 的 称 为 KOALA 的 差 动 驱动 移动 
机 器 人 。 本 书 所 提 方 法 的 一 个 重要 特征 是 在 这 种 独立 系统 中 采用 了 基于 计算 机 
视觉 的 感知 系统 来 构建 地 图 。 该 视觉 系统 采用 了 两 个 摄像 头 在 所 采集 的 图 像 中 
进行 特征 识别 ， 以 及 在 随后 的 图 像 帧 中 进行 特征 跟踪 。 这 种 对 于 场景 标识 和 障 
碍 识别 必 不 可 少 的 系统 有 助 于 开发 更 适合 的 导航 算法 ， 进 行 避 障 和 /或 构建 机 器 
人 导航 的 环境 地 图 。 特 征 跟踪 方法 是 基于 上 一 窗口 与 当前 窗口 的 亮度 差 二 次 方 
和 最 小 来 决定 当前 窗口 是 否 是 滑动 后 过 去 窗口 。 男 外 ,该 系统 还 具有 在 机 融 人 
坐标 中 计算 路 标 3D 距离 的 模块 ， 这 样 就 可 确定 在 地 图 中 的 机 器 人 位 置 ， 并 在 地 
图 中 保存 机 器 人 当前 位 置 和 路 标 坐 标 。 该 系统 已 在 本 实验 室 中 真正 用 于 构建 导 
航 地 图 ， 同 时 在 室内 环境 中 所 构建 的 地 图 具有 较 高 的 精度 。 


8.2 差 动 驱动 KOALA 机 器 人 的 动态 状态 模型 


1E 7.2 节 中 已 详细 介绍 了 基于 EKF 的 SLAM 算法 。 在 此 ， 为 将 该 理论 应 
用 于 实际 的 KOALA 机 器 人 人， 首先 本 节 将 建立 差 动 驱动 的 KOALA 机 器 人 的 动 
态 模 型 。 当 然 这 也 可 从 逻辑 上 扩展 到 其 他 类 似 的 移动 机 器 人 。 这 时 ， 主 要 有 
两 个 独立 变量 决定 了 小 车 的 运动 ， 即 左轮 的 电动 机 转速 和 右 轮 的 电动 机 转速 。 
然而 在 此 分 别 将 机 器 人 几何 中 心 的 直线 位 移 量 和 绕 中 心 垂 直 轴 的 旋转 量 这 两 
个 衍生 变量 看 作 主 导 变 量 。 进 行 域 转换 的 原因 是 由 于 在 旋转 过 程 中 会 严重 变 
形 ， 因 此 如 果 将 旋转 看 作 一 个 变量 ,将 会 引入 误差 。 在线 性、 平移 运动 中 不 
会 存在 这 种 问题 ， 只 是 误差 源 或 不 确定 性 不 同 ， 例 如 ， 里 程 计 编码 器 标定 不 
准确 、 车 轮 旋转 时 打滑 等 。 在 此 ， 假设 机 器 人 不 会 同时 进行 旋转 运动 和 平移 
运动 。 

在 建 模 时 ， 应 注意 到 机 器 人 总 是 沿 圆 弧 运动 的 。 对 于 线性 平移 运动 ， 曲 率 
为 零 ， 而 对 于 旋转 和 运动， 曲率 半径 为 零 。 图 8.1 给 出 了 机 器 人 的 运动 机 制 ， 此 
时 


ABE Sg (8.1) 
0 = 二 = 后 ( 右 轮 旋 转 量 - 左轮 旋转 量 ) (8.2) 
s = (K,/2) ( 右 轮 旋转 量 + 左轮 旋转 量 ) (8.3) 


由 式 (8.2) 和 式 (8.3) 可 得 s， 同 时 ， 根 据 车 轮 编码 器 读数 可 直接 获 得 6。 由 此 可 
得 ，r= 六 和 4C =2rsin 了 。 然 后 ， 当 已 知 机 器 人 的 初始 位 置 $ 时 ， 可 将 AC 分 解 
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图 8.1 KOALA 机 器 人 的 运动 机 制 


dx = 2rsin Leos 


8.4 
dy = 2rsin Dsing ( ) 


db = 0 
在 00" I BL FER, dT r=, WE ar CETERI, dE 


此 ， 当 9<5° 时 ,假设 4C =s。 对 于 KOALA 机 器 人 的 直线 位 移 量 D 和 旋转 量 9， 
可 由 下 式 建立 机 器 人 的 最 终 动态 模型 ， 
Ax = Deosh, xi = X, + Dcos 





Ay = Dsin |y,,, = y, + Dsing (8.5) 
dọ = 6 pi, =o, +8 
由 此 ， 可 计算 雅 各 比 和 矩阵 和 协 方差 矩阵 : 
[óAx 9Ax] 
aD ð du. 
vg = E say =| sind 0 (8.6) 
jah ake 0 1 











- aD 90 - 
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1 0 -Dsing 
V fe, = o l Dcosó | (8.7) 
0 0 l 
cD 0 
Q = à 
| 0 wl 


式 中 oD =D x 单位 位 移 的 标准 偏差 ; 
09=09x 单 位 旋转 的 标准 偏差 。 


8.3 基于 视觉 感知 的 图 像 特征 识别 、 特 征 跟踪 和 每 个 特征 
的 3D 距离 计算 


在 本 书 所 提出 的 SLAM 算法 中 ,利用 视觉 感知 进行 “观测 ”。 所 购买 的 基本 
配置 的 KOALA 机 器 人 中 已 具有 一 些 内 置 传感器 ， 如 车 轮 的 增 量 编码 器 和 红外 传 
感 器 ， 后 来 还 集成 了 一 些 如 声 纳 、 无 线 通信 和 模块、 传感器 云 台 扫描 系统 、 视 觉 
系统 、 控 制 四 个 自由 度 的 伺服 电动 机 、 计 算 平台 等 附件 。 所 有 的 集成 工作 都 是 
在 实验 室内 完成 的 。 图 8.2 给 出 了 专门 用 于 进行 视觉 感知 SLAM 的 集成 模式 下 的 
KOALA 机 器 人 。 


E] 





图 8.2 KOALA 移动 机 器 人 
a) 配置 有 一 些 内 置 传感器 的 原装 机 器 人 b) 在 实验 室 集 成 了 一 些 附 件 的 改装 机 项 人 
在 具体 实现 中 ， 视 觉 感知 采用 了 两 个 网 络 摄像 头 ， 如 图 8.2a 所 示 ， 其 主要 
目的 是 用 于 在 一 个 双 摄 像 头 的 视觉 系统 中 实现 图 像 特 征 选择 ， 并 跟踪 所 选择 的 
特征 和 计算 所 选择 特征 的 3D BSS) 。 根 据 从 每 个 摄像 头 获 得 的 实时 视频 采集 
的 图 像 帧 中 选择 合适 的 图 像 块 或 窗口 来 进行 特征 识别 ， 这 能 够 保证 在 后 续 图 像 
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帧 中 具有 和 较 高 的 跟踪 能 力 。 在 实际 应 用 中 ， 具 有 丰富 边缘 信息 的 图 像 块 更 适用 
于 特征 跟踪 ， 因 此 这 些 图 像 块 (在 本 系统 中 是 静态 的 ) 可 作为 用 于 地 图 构建 的 
最 佳 路 标 。 不 同 视 角 下 (对 于 本 系统 ， 即 左右 两 个 摄像 头 分 别 采 集 的 左 图 和 右 
图 ) 的 特征 匹配 计算 是 获得 深度 信息 所 必需 的 前 提 和 条件。 系统 首先 在 左 摄像 头 
采集 的 图 像 中 执行 一 个 特征 识别 算法 来 确定 合适 的 矩形 图 像 块 或 最 适合 作为 特 
征 跟踪 的 窗口 (具有 足够 纹理 的 图 像 块 ) ， 然 后 在 右 摄像 头 采 集 的 图 像 中 进行 特 
征 跟 踪 。 该 图 像 跟 踪 系 统 的 开发 思想 来 自 于 KLT BR BE ASO? 。 该 跟踪 器 总 是 优 
先 选 择 跟踪 一 个 像素 窗口 或 图 像 块 而 不 是 单个 像素 点 ， 这 是 因为 几乎 不 可 能 跟 
踪 单 个 像素 点 ， 除 非 与 所 有 相 邻 像素 点 之 间 的 亮度 值 完 全 不 同 。 同 时 ， 由 于 像 
素 亮 度 值 会 因 存在 噪声 而 变化 ， 因 此 跟踪 结果 也 会 非常 混乱 。 由 此 ,在 亮度 曲 
线 分 布 的 基础 上 ， 通 过 在 一 幅 图 像 帧 中 特征 之 间 的 距离 最 小 来 选择 N 个 特征 窗 
口 。 对 于 一 个 二 维 图 像 函 数 1 (x，y) ， 其 梯度 是 一 个 矢量 ， 且 每 个 窗口 G 的 梯 
度 可 沿 x 和 yy 方向 进行 计算 : 
Bu By gs RE, 
Eo ui FA g, | 
根据 计算 和 矩阵 G 的 特征 根 A， 和 A, 来 评估 所 选 窗口 是 否 适 合作 为 一 个 特征 
窗口 ， 若 


G=| (8.8) 


min(A,,A,) >A (8.9) 
则 可 作为 特征 窗口 。 其 中 ，A d&— 47 BE RL) 。 两 个 较 小 的 特征 根 意 味 着 窗 
口中 的 亮度 曲线 分 布 大 致 相同， 而 一 个 较 大 和 一 个 较 小 的 特征 根 则 对 应 着 一 个 
单 向 纹理 模板 。 另 一 方面 ， 两 个 较 大 的 特征 根 表示 是 角 点 或 椒盐 纹理 。 
一 旦 特征 选择 完成 ， 接 下 来 就 要 在 图 像 序列 中 一 帧 帧 地 跟踪 这 些 特 
征 "-"”。 类 似 于 参考 文献 [11] ， 在 此 计算 使 得 两 帧 图 像 中 窗口 间 图 像 亮 度 差 
的 二 次 方 和 最 小 的 一 个 特征 窗口 的 中 心 位 移 dp = [dxp，dyp] 。 帧 间 运 动 较 小 
时 ,两 帧 图 像 中 特征 的 运动 可 由 一 个 纯 位 移 模型 近似 。 然 而 帧 间 运 动 较 大 时 ， 
由 线性 变形 和 纯 位 移 构成 的 一 个 仿 射 模型 被 认为 更 准确 。 在 此 ， 根据 其 中 包括 
一 个 表示 图 像 帧 中 线性 变形 的 仿 射 运动 模型 以 及 特征 平移 的 形变 矩阵 的 差异 度 
测量 ， 跟 踪 过 程 中 特征 跟踪 的 质量 更 高 。 图 像 中 像素 点 的 运动 可 由 下 式 表示 : 





J(Axp + dp) = I(xp) (8. 10) 
式 中 ”J 一 一 当前 图 像 ; 
I 原始 图 像 ; 
A=1+D (7 为 一 个 2 x2 的 单位 矩阵 ， 为 形变 矩阵 ); 
dp— FERE.» 


由 此 ， 可 利用 w (xp), ， 即 一 个 参考 文献 [11] 中 的 权 值 函数 (通常 选取 一 
个 单位 函数 或 高 斯 函数 来 强调 窗口 中 心 区 域 ) 来 计算 差异 度 : 
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e = [i s(Azp + ap) _ I( xp) ]'w( xp) dxp (8.11) 


利用 两 个 窗口 图 像 亮 度 值 之 间 的 牛顿 一 拉夫 还 ( Newton-Raphson) 最 小 化 算 

法 来 迭代 搜索 一 帧 新 的 图 像 中 新 的 特征 窗口 中 心 点 位 置 ， 接 下 来 需 对 系统 进行 
求解 来 获得 dp : 

Gdp =e (8. 12) 


RH G = [(eg'u)da,G = 二 阶 加 权 系数 矩阵 (2 x 2); 
e = 加 权 亮 度 值 误 差 矢 量 (2 x 1) ( = ( [t= Dente]; 


dp = 平移 矢量 (2x1) (dp= [dxp dypl"); 


g = 梯度 矢量 (2x1) (s-[5 ay). 


akaf" | nd = [Jute stare 


像 中 新 的 窗口 中 心 来 求解 式 (8.12)， 在 本 次 迭代 中 对 x， racked = X, uasa + dXp . 
Yp_tacked = p_trackea + dyp 进行 跟踪 o 

根据 有 有 关 所 用 摄像 头 和 云 台 几何 形状 的 数据 ， 可 得 到 所 跟踪 路 标的 3D HE 
BCP 。 为 获得 立体 视觉 的 深度 信息 ， 需 要 两 个 摄像 头 的 视线 在 场景 中 的 已 点 
相交 ， 然 后 根据 该 信息 ， 可 获得 世界 坐标 系 (WCS) 中 所 观测 的 场景 点 的 三 维 
坐标 信息 。 本 系统 中 的 距离 计算 模块 基于 Andrew J. Davison 所 提出 的 小 孔 成 像 模 

型 ， 即 利用 了 常用 的 摄像 头 标 定 和 矩阵 和 透视 投影 方程 ， 并 结合 “最 近 中 点 
法 ”。 图 8.3 给 出 了 本 实验 室 所 设计 和 开发 的 主动 云 台 的 正视 图 ， 其 中 五 = 云 台 
中 心 与 地 面 的 垂直 距离 , T= 左 摄像 头 聚焦 轴 和 右 摄 像 头 聚焦 轴 之 间 的 水 平 距离 ， 
c= 俯仰 轴 与 摄像 头 光 轴 交点 之 间 每 个 聚焦 轴 的 偏 移 量 。 

一 旦 在 左 、 右 摄像 头 图 像 中 识别 并 跟踪 到 新 路 标 或 图 像 块 ， 就 可 利用 基于 
EKF 的 SLAM 算法 中 新 路 标的 初始 化 步骤 在 地 图 中 进行 初始 化 。 类 似 地 ， 左 、 
右 摄像 头 图 像 中 图 像 块 的 识别 与 跟踪 〈 即 在 之 前 位 置 上 机 器 人 采集 的 图 像 中 所 
识别 的 图 像 块 ) 将 构成 基于 EKF 的 SLAM 算法 中 的 重新 观察 步 又。 在 该 步骤 中 ， 
根据 卡尔 曼 滤波 的 “预测 ”步骤 来 计算 路 标点 的 估计 位 置 ， 然 后 通过 执行 卡尔 
曼 滤波 算法 中 的 “观察 和 更 新 ”步骤 来 进一步 修正 。 

基于 视觉 感知 的 EKF-SLAM 算法 的 具体 实现 步骤 如 算法 8.1 所 示 。 在 此 ， 
要 求 机 器 人 经 过 某 些 导航 点 ， 并 直接 构建 其 周围 的 环境 地 图 。 为 实现 该 功能 ， 
机 器 人 应 先 运 动 一 定 距 离 ， 并 采集 几 帧 图 像 来 同时 进行 路 标 观测 和 自 定位 。 为 
构建 机 器 人 前 方 、 左 侧 和 右 侧 的 环境 地 图 ， 分别 在 水 平角 0°*、+0°* 和 一 0° 位置 
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图 8.3 本 实验 室 设计 的 包括 传感器 扫描 云 台 系统 、 两 个 网 络 摄像 头 和 四 个 
用 于 四 自由 度 独立 控制 (水 平 控制 、 俯 仰 控制 、 左 摄像 头 聚焦 控制 和 右 摄像 
头 聚 焦 控 制 ) 的 伺服 电动 机 的 主动 云 台 的 前 视图 





处 采集 图 像 。 由 此 ， 在 EKF 算法 的 观测 步骤 中 ， 机 器 人 在 其 前 方 、 左 侧 和 右 侧 
的 环境 中 识别 并 获取 特征 /路 标 。 在 机 器 人 的 前 向 运动 过 程 中 ,通过 视觉 传感器 
在 几 个 水 平角 方向 上 获取 和 跟踪 路 标 来 执行 “预测 ”步骤 ， 并 以 迭代 形式 执行 
EKF 算法 的 “校正 和 更 新 ” 步 台 ， 直 到 到 达 最 后 一 个 导航 点 。 在 最 后 一 次 迭代 
中 所 构建 的 地 图 看 作 可 应 用 于 在 同一 环境 中 执行 其 他 任务 时 机 器 人 所 构建 的 最 
终 地 图 。 

算法 8.1 在 室内 环境 下 (本 实验 室 ) ， 利 用 视觉 传感器 在 KOALA 机 器 人 
上 实现 的 基于 EKF 的 实际 SLAM 算法 





步骤 1 指定 机 器 人 导航 所 需 的 导航 点 和 机 器 人 初始 位 姿 。 

GR 2 机 带 人 运动 一 段 距离 ， 并 执行 EKF 算法 的 预测 步骤 。 

JE 在 水 平角 0? 处 从 左 、 右 两 个 摄像 头 的 连续 视频 序列 中 采集 图 像 ， 并 进行 
特性 识别 、 特 征 跟 踪 和 计算 特征 点 与 机 天 人 之 间 的 距离 

GRA 在 水 平角 +9° 处 ,重复 执行 步骤 3 

步骤 5 在 水 平角 -09° 处 ,重复 执行 步骤 3 
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步骤 6 对 于 步骤 3 ~ APR S 所 观测 到 的 新 特征 /路 标 ， 在 地 图 中 进行 初始 化 。 

步骤 7 对 于 步骤 3 ~ 步骤 5 观测 到 的 新 特征 /路 标 ， 执 行 EKF 算法 的 观察 与 更 
新 步 又 ， 并 修正 已 构建 的 地 图 。 

步 又 8 执行 步骤 2-~ 步 又 7， 直 到 机 器 人 到 达 最 后 一 个 导航 点 。 

步骤 9 保存 机 器 人 所 构建 的 最 终 地 图 ， 并 作为 环境 地 图 。 


8.4 实际 性 能 评估 


如 上 所 述 ，KOALA 机 器 人 的 大 小 为 32cm x32cm， 是 由 瑞士 K-team 公司 研 
制 的 六 轮 差 动 驱 动 小 车 。 在 前 面 的 章节 中 已 介绍 了 KOALA 机 器 人 中 的 硬件 控制 
是 由 车 载 微 控制 器 (16MHz Motorola 68331@ 22MHz) 实现 的 ”。 为 在 机 器 人 系 
统 中 增加 水 平 、 俯 仰 、 左 摄像 头 聚 焦 和 右 摄 像 头 聚焦 四 个 自由 度 ， 开 发 了 一 个 
基于 PIC 16F876A 微 控 制 器 的 系统 ， 该 系统 工作 在 中 断 驱 动 模 式 ， 并 与 Motorola 
控制 器 构成 一 个 主 / 从 配置 。 这 种 用 于 与 实际 机 器 人 外 设 接口 的 PIC 微 控制 器 系 
统 能 够 大 大 提高 实际 应 用 的 灵活 性 ， 有 具体 开发 过 程 已 在 第 2 章 中 详细 介绍 。 

图 8.4 给 出 了 一 个 要 求 机 器 人 通过 几 个 指定 导航 点 进行 导航 并 通过 执行 基 
于 视觉 的 SLAM 算法 来 构建 地 图 的 室内 环境 。 为 评价 系统 性 能 ， 在 此 绘制 了 一 
个 包含 100 个 方块 ， 其 中 每 个 方块 大 小 为 20cm x20em 的 栅 格 ， 即 2m x2m 的 导 

图 8.5 给 出 了 实验 室 环境 下 实际 执行 EKF-SLAM 算法 时 的 GUI 软件 界面 。 
图 8.5 的 不 同 图 像 中 给 出 了 在 利用 EKF-SLAM 算法 构建 增 广 地 图 的 迭代 过 程 中 
所 识别 的 路 标 ， 并 集成 在 所 构建 的 地 图 中 。 点 线 表示 机 器 人 导航 中 增加 路 标点 
的 理想 路 径 。“ 淡 蓝 色 三 角 ” 表 示 机 器 人 的 初始 位 置 ， 如 图 8.4 所 示 ， 在 实现 过 
程 中 该 初始 位 置 为 (z, x, 中) = (-100, 0, 0) 。 对 于 此 处 的 具体 实现 ， 符 
Fz 和 中 与 参考 文献 [3] 中 所 用 的 符号 一 致 。 因 此 ，z 方向 和 x 方向 分 别 对 
应 x 方 向 和 Y 方 向 ， 正 如 理论 中 所 指定 的 一 样 。 在 导航 过 程 中 ， 机 器 人 识别 其 周 
围 环境 中 的 路 标 ， 并 初始 化 其 位 置 或 在 地 图 中 修正 其 位 置 。 随 着 机 器 人 不 断 前 
进 ， 识 别 的 路 标 个 数 以 及 地 图 大 小 也 不 断 增 加 。 地 图 中 的 “十 字 ” 表 示 所 识别 
的 路 标的 2D 位置。 图 8. 5d 给 出 了 KOALA 机 器 人 测试 运行 后 得 到 的 最 终 地 图 。 

图 8.6 给 出 了 实际 中 采集 图 像 的 GUI 软件 开发 界面 ， 包括 KOALA 机 器 人 的 
位 置 表示 、 特 征 提取 和 跟踪 算法 的 性 能 演示 以 及 路 标识 别 等 。 实 线 方块 中 的 图 
像 块 看 作 新 路 标 ， 虚 线 方块 中 的 图 像 块 看 作 重 新 观察 的 路 标 。 界 而 中 还 显示 了 
每 个 跟踪 路 标 与 机 器 人 的 3D 计算 距离 。 
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图 8.4 机 器 人 导航 并 执行 EKF SLAM 算法 
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图 8.5 EKF-SLAM 算法 的 不 同 
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中 用 于 构建 地 图 的 真实 路 标识 别 
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图 8.6 KOALA 机 器 人 在 环境 中 测试 运行 过 程 中 ， 在 一 些 有 代表 性 的 位 置 上 进行 
特征 提取 、 特 征 跟踪 以 及 所 跟踪 特征 与 机 器 人 之 间 3D 距离 计算 的 实验 结果 

图 8.7 给 出 了 实际 环境 中 特征 /路 标识 别 与 跟踪 的 三 种 示例 。 网 格 中 和 垂直 
方向 上 的 线 以 及 点 有 助 于 标识 和 获得 环境 中 的 实际 位 置 。 空 心 圆圈 表示 相对 于 
环境 中 识别 的 图 像 块 的 实际 对 象 。 所 构建 的 地 图 中 这 些 路 标的 估计 位 置 如 图 8.5 
所 示 ， 表 明 在 实际 2D 位 置 与 地 图 中 大 多 数 估计 2D 位置 之 间 具 有 较 小 的 偏差 ， 
然而 这 在 实际 实验 中 是 可 以 理解 的 。 表 8.1 给 出 了 三 种 情况 下 这 些 路 标的 实际 
位 置 和 估计 位 置 ， 如 图 8.7 所 示 。 
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表 8.1 对 于 图 8.7 所 示 的 三 个 实际 路 标 ， 比 较 基于 视觉 感知 的 EKF-SLAM 算法 的 性 能 
估计 位 置 /em 实际 位 置 /em 


z 坐标 x 坐标 z 坐标 x 坐标 


SI 编号 路 标 描述 








图 8.6% a A 8.6% b 中 的 路 标 (键盘 的 左 
= a 和 图 Ph 的 路 标 (键盘 的 左下 ag 36 E e 
1) 





8.6% c 和 图 8.6 d 中 的 路 标 (UMAX 盒子 中 
RE “A” WEF) 


4 








图 8.67e 和 图 86°F Hy Bi (FOXIN 盒子 中 
粗 条 线 的 右上 角 ) 




















D “此 处 原 书 有 误 ， 应 为 图 8.7。 一 一 译 者 注 
8.5 本 章 小 结 


本 章 介绍 了 室内 环境 下 利用 基于 两 个 网 络 摄像 头 的 视觉 感知 机 制 的 移动 机 
tt A SLAM 的 理论 与 具体 实现 。 该 系统 体现 了 一 个 算法 的 成 功 实 现 ， 首 先 从 安 
装 在 自主 开发 的 KOALA 移动 机 带 人 主动 云 台 上 的 两 个 摄像 涉 所 获得 的 实时 视频 
采集 图 像 帧 中 进行 图 像 特征 识别 ; 然后 在 后 续 图 像 帧 中 跟踪 当前 图 像 帧 中 所 识 
别 的 特征 /路 标 ， 并 集成 在 所 构建 的 地 图 中 ; 最 后 在 从 机 咒 人 坐标 系 中 计算 的 路 
标 距离 基础 上 ， 利 用 3D 距离 计算 模块 来 计算 路 标 在 世界 坐标 系 中 的 实际 坐标 。 
该 系统 成 功 地 在 实验 室 环 境 下 进行 了 测试 运行 ， 实 验 表 明 地 图 中 的 路 标 估计 位 
置 与 实际 路 标 位 置 之 间 的 偏差 很 小 。 希望 这 种 成 功 实 现 能 够 启发 更 多 的 读者 在 
更 复杂 环境 下 和 室外 环境 下 实现 类 似 的 地 图 构建 系统 。 

致谢 本 章 所 介绍 的 研究 工作 得 到 了 印度 技术 教育 委员 会 RPS 项 目的 资助 
(资助 号 : 8023/BOR/RPS-89/2006-07 ) 。 
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